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LỜI NÓI ĐẦU 


Thuật ngữ robot xuất hiện lần đâu tiên năm 1921 để chỉ một “nhân vật viễn 
tưởng ” có khả năng làm việc mêm dẻo nhưng khỏe gấp nhiều lần con người. “Nhân 
vật đó ” đó đã là hiện thực vào năm 1960 và năm 1961. Robot công nghiệp đã được 
ứng dụng trong công nghiệp. Trong dây chuyên sản xuất với mức độ tự động hóa 
cao, robot công nghiệp đóng vai trò rất quan trọng trong việc giảm cường độ lao 
động cho người lao động, tăng năng suất và độ chính xác gia công, góp phần tăng 
chất lượng và số lượng và giảm giá thành sản phẩm . 

Cuốn sách “Điều khiển Robot công nghiệp” có nội dung là phân tích, tính 
toán cơ cấu cơ khí robot, thiết lập mô hình toán học, phân tích các cấu trúc và 
phương pháp thiết kế hệ thống điều khiển cho robot. 

Cuốn sách được biên soạn theo để cương môn học Robot của chuyên ngành Tự 
động hóa - Trường Đại học Bách khoa Hà Nội đã được Hội đông Khoa học Trường 
thông qua. : 

Với mục đích biên soạn cho giảng dạy môn học Robot Trường Đại học Bách 
Khoa Hà Nội và các Trường Đại học, Cao đẳng khác. Sách gồm 9 chương được sắp 
xếp theo trình tự hợp lý, có nội dung cơ bản và chuyên sâu trong lĩnh vực kỹ thuật 
robot công nghiệp. Trong mỗi chương có các ví dụ mình họa và bài tập tự giải. 

Chương 1 - Tổng quan về robot công nghiệp: Trình bày một cách tổng quan 
về lịch sứ phát triển, các đặc tính robot công nghiệp và các khâu của một robot 
công nghiệp với mô tả chỉ tiết về cấu tạo cơ cấu chuyển động. 

Chương hai - Động học vị trí robot: Trình bày cơ sở quan trọng của động học 
vị trí là phép biến đối, mối quan hệ vị trí giữa các bộ phận trọng robot thể hiện 
bằng hai bài toán động học thuận và động học ngược. 

Chương 3 - Động học vị trí vì sai: Đề cập những vấn đề về chuyển động vi sai 
của robot, quan hệ dịch chuyển vì sai giữa các bộ phận robot và ma trận Jacobien 
trong robot. 

Chương 4 - Động lực học robot: Phân tích và xây dựng phương trình động lực 
học của robot đơn giản và robot n thanh nối. Xây dựng các dạng mô hình toán học 
của hệ thống robot và quan hệ mômen và lực tĩnh trong robot. 

Chương 5 - Thiết kế quỹ đạo chuyển động: Phân tích cơ sở và các phương 
pháp xây dựng quỹ đạo chuyển động robot ở không gian khớp và không gian tay với 
một số dạng quỹ đạo thông dụng. 


Chương 6 - Điêu khiển chuyển động robot: Phân tích cấu trúc và phương 
pháp tổng hợp các hệ thống điêu khiển chuyển động trong không gian khớp: Hệ 
thống điều khiển độc lập, hệ thống điêu khiển tập trung, hệ thống điêu khiển thích 
nghỉ và hệ thống điêu khiển không gian tay sử dụng ma trận Jacobien chuyển vị và 
nghịch đảo. Đánh giá chất lượng hệ thống điều khiển thông qua các kết quả mô 
phỏng. 

Chương 7 - Hệ thống điêu khiển lực: Trình bày về các phương pháp điều 
khiển lực kinh điển: Điều khiển trở kháng và điêu khiển hỗn hợp với một số kết quả 
mô phỏng. 

Chương 8 - Cơ cấu chấp hành và cấu trúc hệ thống điều khiển: Phản tích 
các đặc điểm và cấu trúc của các hệ thống truyền động sử dụng trong robot công 
nghiệp: hệ thống thủy lực, khí nén và hệ thống truyền động điện. Mô tả cấu trúc 
chung của hệ thống điều khiển của robot. 

Chương 9 - Cảm biển trong robot: Mô tả về cấu tạo, nguyên lý làm việc, đặc 
tính và ứng dụng của các cảm biến sử dụng trong robot gôm các cảm biến bên 
trong và cảm biến bên ngoài. 

Sách được dùng làm giáo trình chính cho chuyên ngành Tự động hóa, cũng có 
thể làm tài liệu học tập cho sinh viên một số ngành trong ngành Điện và một số 
ngành liên quan, làm tài liệu tham khảo cho học viên cao học, nghiên cứu sinh 
chuyên ngành Tự động hóa - Điêu khiển tự động, cho các cán bộ kỹ thuật trong vận 
hành, thiết kế và bảo dưỡng các hệ thống điêu khiển robot trong công nghiệp. 

Tác giả bày tỏ lòng biết ơn về những định hướng của Giáo sư Nguyễn Công 
Hiển cho cuốn sách này. Tác giả cẩm ơn PGS.TS. Nguyễn Văn Liễn đã đọc và góp 
ý cho nội dụng của cuốn sách, cảm ơn các đồng nghiệp ở Bộ môn Tự động hóa xí 
nghiệp công nghiệp - Trường Đại học Bách khoa Hà Nội đã khuyến khích, động 
viên tác giả và có đóng góp cho nội dụng của cuốn sách. Tác giả đặc biệt cảm ơn 
vợ và gia đình đã dành cho tác giả mọi thời gian tự do và khuyến khích, động viên 
giúp đỡ trong thời gian viết cuốn sách này. 

Tuy đã dành nhiều thời gian suy nghĩ và biên soạn, song với sự biên soạn lần 
đầu, cuốn sách không thể tránh khỏi các thiếu sót. Tác giả xin chân thành cảm ơn 
và mong nhận được sự đóng góp ý kiến của bạn đọc. 

Các ý kiến đóng góp xin gửi về Bộ môn Tự động hóa xí nghiệp công nghiệp - 
Trường Đại học Bách khoa Hà Nội hoặc Nhà xuất bản Khoa học và Kỹ thuật - 70 
Trần Hưng Đạo, Hà Nội. 
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CHƯƠNG 1 
TỔNG QUAN VỀ ROBOT CÔNG NGHIỆP 


1.1.Tự động hoá và robot công nghiệp 


Thuật ngữ robot có nguồn gốc từ khoa học viễn tưởng. Từ "Robota" lần đầu 
tiên xuất hiện năm 1920 trong một vở kịch của nhà viết kịch người Tiệp Karel 
Capek, trong đó ông đã mô tả một "nhân vật" có thể ứng xử như con người, có 
khả năng làm việc khoẻ gấp đôi con người, nhưng không có cảm tính, cảm giác 
như con người. Sự phát triển của lĩnh vực robot bất đầu từ 40 năm sau đó. 

Robot được định nghĩa đưới dạng các khía cạnh khác nhau. Robot được coi 
là một tay máy có một vài bậc tự do, có thể được điều khiển bằng máy tính. Một 
định nghĩa khác về robot công nghiệp hiện nay được chấp nhận là: Robot công 
_ nghiệp là một cơ cấu cơ khí có thể lập trình được và có thể thực hiện những 
công việc có ích một cách tự động không cần sự giúp đỡ trực tiếp của con 
người. Hiệp hội những nhà chế tạo - nhà sử dụng đưa ra định nghĩa robot như 
sau: Robot là một thiết bị có thể thực hiện được các chức năng bình thường như 
con người và có thể hợp tác nhau một cách thông minh để có được trí tuệ như 
con người. Trong Bách khoa toàn thư mới (phiên bản 7.0 1995) viết: "Robot có 
thể định nghĩa là một thiết bị tự điều khiển hoàn toàn bao gồm các bộ phận điện 
tử, điện và cơ khí,..." 

Tự động hoá (Automation) và kỹ thuật robot (Robotics) là hai lĩnh vực có 
liên quan mật thiết với nhau. Về phương điện công nghiệp, tự động hoá là một 
công nghệ liên kết với sử dụng các hệ thống cơ khí, điện tử và hệ thống máy 
tính trong vận hành và điều khiển quá trình sản xuất. Ví dụ, dây chuyển vận 
chuyển, các máy lắp ráp cơ khí, các hệ thống điểu khiển phản hồi, các máy 
công cụ điều khiển chương trình số và robot. Như vậy, có thể coi robot là một 
._ dạng của thiết bị tự động hoá công nghiệp. 

Có ba loại hệ thống tự động hoá công nghiệp: Tự động hoá cế định, tự 
động hoá lập trình và tự động hoá linh hoạt. Tự động hoá cố định được sử dụng 
ở những dây chuyên sản xuất với số lượng sản phẩm lớn, do đó cân thiết kế các 
thiết bị đặc biệt để sản xuất các sản phẩm với số lượng lớn và hiệu suất rất cao. 


Công nghiệp sản xuất ôtô có thế coi là một ví dụ điến hình. Tính kinh tế của tự 
_ động hoá cố định khá cao do giá thành thiết bị chuyên dụng được chia đều cho 
số lượng lớn các đơn vị sản phẩm, dẫn đến giá thành trên một đơn vị sản phẩm 
thấp hơn so với các phương pháp sản xưất khác. Tuy nhiên vốn đầu tư của hệ 
thống tự động hoá cố định cao, do đó nếu số lượng sản phẩm nhỏ hơn thiết kế, 
giá thành sản phẩm sẽ rất cao. Mặt khác, các thiết bị chuyên dùng được thiết kế 
cho sản xuất một loại sản phẩm, sau khi chu kỳ sản phẩm kết thúc, các thiết bị 
chuyên dụng đó sẽ trở thành lạc hậu. 

Tự động hoá lập trình được sử dụng ở quá trình sản xuất với sản phẩm đa 
dạng và số lượng sản phẩm tương đối thấp. Trong hệ thống tự động hoá này, các 
trang thiết bị sản xuất được thiết kế để thích nghỉ với các đạng sản phẩm khác 
nhau. Chương trình sẽ được lập trình và được đọc vào các thiết bị sản xuất ứng 
với các loại sản phẩm cụ thể. Về khía cạnh kinh tế, giá thành trang thiết bị lập 
trình có thể phân bộ cho số lượng lớn sản phẩm, ngay cả với các loại sản phẩm 
khác nhau. 

Tự động hoá linh hoạt hoặc hệ thống sản xuất linh hoạt (FMS), hệ thống 
sản xuất tích hợp máy tính (hình 1.1). Ý tưởng của dạng tự động hoá lình hoạt 
mới được pháp triển và áp dụng vào thực tế quãng 20-25 năm cho thấy phạm vi 
ứng dụng thích hợp nhất đối với quá trình sản xuất có số lượng sản phẩm trung 
bình. Dạng tự động hoá linh hoạt sẽ bao gồm các đặc điểm của hai dạng tự động 
hoá cố định và lập trình. Nó cần được lập trình cho các loại sản phẩm khác 
nhau, nhưng số dạng sản phẩm khác nhau sẽ hạn chế hơn loại tự động hoá lập 
trình. Hệ thống sản xuất bao gồm nhiều trạm làm việc đặt nối tiếp nhau trong 
một dây chuyền. Máy tính trung tâm và hệ thống điều khiển trung tâm sẽ điều 
khiển đồng thời các trạm hoạt động. 


Số lượng sản phẩm 
15000 







TĐH linh hoạt 


'TEH lập trình 





1 3 9 lŠ 30 ¡00 1000 Số loại sản phẩm 


Hình 1.1. Quan hệ số loại và số lượng sản phẩm ứng với các dạng tự động hoá, 


Robot có liên quan mật thiết với tự động hoá lập trình. Robot là một máy 
có khả năng lập trình và có một số đặc tính như con người. Robot có thể được 
lập trình để đi chuyển cánh tay thông qua các trình tự chuyển động có tính chu 
kỳ để thực hiện các nhiệm vụ khác nhau. Ví dụ, các máy bốc dỡ hàng, robot 
hàn, sơn... robot cũng được sử dụng rộng rãi trong hệ thống sản xuất lính hoạt 
hoặc thậm chí trong hệ thống tự động hoá cố định. Hệ thống này gồm một số 
máy, hoặc các robot làm việc cùng nhau được điều khiển bằng máy tính hoặc 
bộ điều khiển lập trình. Ví dụ, đây chuyền hàn vỏ ôtô gồm nhiều cánh tay robot 
có nhiệm vụ hàn các bộ phận khác nhau. Chương trình lưu trữ trong máy tính 
được nạp cho từng robot làm việc ở mỗi bộ phận của dây chuyền hàn ôtô. Như 
vậy đây là một dây chuyển sản xuất linh hoạt với mức độ tự động hoá cao. 


1.2. Lịch sử phát triển của Robot 


Bảng I.1 trình bày tóm tắt những điểm mốc chính của lịch sử phát triển 
công nghệ robot. Từ thế kỷ 17, một số thiết bị máy móc được chế tạo đã có 
một số đặc tính làm việc như robot công nghiệp hiện nay. Jacques dc 
Vancanson đã chế tạo một vài "búp bê nhạc sĩ”. Năm 1805 Henri Maillarder đã 
chế tạo những con búp bê cơ khí có khả năng vẽ tranh. Trong các đồ chơi đó, 
một số cơ cấu cam được sử dụng như một chương trình điều khiển thiết bị vẽ và 
viết. Trơng cuộc cách mạng công nghiệp, có một số phát minh cơ khí khác 
trong lĩnh vực đệt, điển hình là khung đệt vải của ƒacquard (1801). 


Bảng 1.1. Một số điểm mốc của lịch sử phát triển công nghệ robot 
Sự kiện quan trọng 
Giữa thế J. đe Vancanson chế tạo một số búp bê có khí đánh nhạc. 
kỷ 17 
1801 J. Jacquard phát minh khung dệt vải có thể lập trình 
1805 H. Maillader chế tạo búp bê cơ khí biết vẽ tranh. 
1892 S. Babbitt (Mỹ) đã thiết kế một cần trục truyền động động cơ có 


cơ cấu kẹp để gắp thỏi thép đúc ra khỏi lò nung. 

1938 W. Pollard và H. Roselund (Mỹ) đã thiết kế một cơ cấu phun 
sơn lập trình cho Công ty DeVilbiss 

1946 G.C. Devol (Mỹ) sáng chế thiết bị điều khiển có thể ghi lại 
những tín hiệu điện bằng từ hoá, sau đó được sử dụng để điều 
khiển một máy cơ khí. R 








Cơ cấu tay máy điều khiển từ xa có thể mang các vật liệu phóng 
xạ được chế tạo. 

Mẫu máy điều khiển số đầu tiên được trưng bày ở Viện Công 
nghệ Massachusetts sau một vài năm nghiên cứu chế tạo. 

G.C. Devol đăng ký bản quyền phát minh thiết kế robot. 

Robot "Unimate" đầu tiên được giới thiệu là robot truyền động 
thuỷ lực; nó sử dụng dạng nguyên lý điều khiển số cho điều 
khiển cơ cấu tay máy. 

Công ty Ford lấp đặt Robot Unimate. 

Công ty General Motor (GM) lắp đặt robot công nghiệp đầu tiên 
(robot Ứnimate) trong dây chuyên sản xuất 

Công ty Trallfa (Nauy) lắp đặt robot phun sơn. 

Robot di chuyển "Shakey" được chế tạo tại Viện Nghiên cứu 
Stanford (Mỹ). Robot này được trang bị một số cảm biến tiếp 
xúc, máy ảnh, có thể đi chuyển trên mặt sàn. 

Tay máy Stanford là robot nhỏ điều khiển bằng điện được chế 
tạo ở Trường Đại học Stanford (Mỹ). 

Hiệp hội robot công nghiệp Nhật Bản (JIRA) bắt đầu đề xuất sử 
dụng robot trong công nghiệp Nhật Bản. 

Viện Nghiên cứu Sianford (Mỹ) công bố ngôn ngữ lập trình 
máy tính đầu tiên cho robot trên là ngôn ngữ WÀ VE, 

| Công ty Cincinnati Milacron giới thiệu Robot T° điều khiển 
bằng máy tính. 

Robot "Sigma” được sử dụng trong công nghiệp lắp ráp - là một 
trong ứng dụng robot trong dây chuyên lắp ráp đầu tiên, 

Phòng thí nghiệm Charies Stack Draper (Mỹ) đã chế tạo cơ cấu 
nhún có tâm ở xa sử dụng cho robot lắp ráp. 

Robot PUMA (máy lắp ráp vạn năng có thể lập trình) được trình 
diễn. 

Robot TỶ của Hãng Cincinnati Milacron được lập trình thực hiện 
các công việc khoan và hàn trên các bộ phận của máy bay. 
Robot SCARA được sáng chế cho dây chuyển lắp ráp ở Trường 
Đại học Yamanashi (Nhật). Một số robot thương mại này được 
giới thiệu vào năm 1981. 
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Robot công nghiệp bắt đầu một thời kỳ phát triển nhanh, với 
các robot mới. 

Robot truyền động trực tiếp (DDR) được sáng chế ở Trường Đại 
học Carnegie-Mellon. 

Hãng máy tính TBM chế tạo Robot RS-1 cho lắp ráp. 

Một số hệ thống lập trình "OFF-line” được trình diễn cho robot. 
Úng dụng robot tiếp tục phát triển mạnh tập trung vào tích hợp 
robot trong các đây chuyển sản xuất linh hoạt (FMS) và hệ 
thống CIM. 

Bước phát triển mới của nền kinh tế thế giới với tập trung sản 
xuất các sản phẩm phức tạp và ứng dụng công nghệ vi điện tử 
và công nghệ hiển thị trong robot của dây chuyền sản xuất tự 
động hóa. 

















Vào những năm đầu thế kỷ 20, điều khiển số và cơ cấu điều khiển từ xa là 
hai công nghệ quan trọng trong sự phát triển của robot. Điều khiển số 
.. Numrical Control) được phát triển cho máy công cụ cuối năm 1940, đầu năm 
1950. Năm 1952, Viện Massachusetts (MIT, Mỹ) đã trình diễn máy phay 3 trục 
điều khiển số. Lĩnh vực điều khiển xa liên quan với việc sử dụng cơ cấu điều 
khiển xa (Teleoperator). Đó là một cơ cấu cơ khí thực hiện chuyển động ở 
khoảng cách xa dưới sự điều khiển của con người. Các cơ cấu tự động này được 
chế tạo và sử dụng để mang các vật chất độc hại như chất phóng xa vào các năm 
1940. 

Robot hiện đại là sự kết hợp của kỹ thuật điều khiển số và cơ cấu tự động 
điều khiển xa. Thực vậy, robot là một cơ cấu cơ khí với chuyển động được điều 
khiển bằng kỹ thuật lập trình tương tự như đã sử dụng trong các máy điều khiển 
số. Cyril Walter Kenward (người Anh) là một nhà phát minh đầu tiên đã đăng 
ký bản quyên cho thiết bị Robot vào tháng 3/1954 và phát minh của Ông đã 
được thực hiện vào năm 1957. Người thứ hai là nhà phát minh George C. Devol 
(người Mỹ) đã có hai phát minh góp phần vào sự phát triển của robot hiện đại 
ngày nay. Đó là một thiết bị ghi các tín hiệu điện bằng từ hoá và sử dụng cho 
điều khiển các máy. Thiết bị này đã được chế tạo và công bố vào năm 1952. 
Phát minh thứ hai là "Thiết bị truyền bài báo được lập trình” được công bố vào 
năm 1961. Với sự hợp tác nghiên cứu của IJoseph Engelberger là Chủ tịch Công 


H 


ty Unimate và George €. Devol, robot Unimate là robot công nghiệp đầu tiên đã 
được xuất hiện trên thị trường năm 1962 ở Mỹ. 

Những năm sau đó, nhiều đóng góp giá trị về ngôn ngữ lập trình robot đã 
đánh dấu những bước phát triển quan trọng của robot hiện đại. Đó là các công 
trình nghiên cứu về ngôn ngữ lập trình hướng đối tượng cho robot của Viện 
Nghiên cứu Stanford: Ngôn ngữ thực nghiệm WAVE(1973) và ngôn ngữ AL 
(năm 1974). Ngôn ngữ VAL của Công ty Unimate là ngôn ngữ lập trình robot 
thương mại đầu tiên. 


1.3. Các đặc tính của robot công nghiệp 


1.3.4. Tải trọng: Tải trọng là trọng.lượng robot có thể mang và giữ trong 
khi vẫn đảm bảo một số đặc tính nào đó. Tải trọng lớn nhất lớn hơn tải trọng 
định mức nhiều, nhưng robot không thể mang tải trọng lớn hơn định mức, vì khi 
đó robot không đảm bảo được độ chính xác di chuyển. Tải trọng robot thông 
thường rất nhỏ hơn trọng lượng robot. Ví dụ, robot LR Mate của hãng Fanuc có 
trọng lượng 40 kg chỉ mang được tải trọng 3 kg; robot M-16i có trọng lượng 
269 kg mang được tải trọng 15,8 kg. 

1.3.5. Tầm với: Là khoảng cách lớn nhất robot có thể vươn tới trong phạm 
vi làm việc. Tầm với là một hàm phụ thuộc vào cấu trúc của robot. 


1.3.6. Độ phân dải không gian: Là lượng gia tăng nhỏ nhất robot có thể 
thực hiện khi di chuyển trong không gian. Độ phân dải phụ thuộc vào độ phân 
dải điều khiển và độ chính xác cơ khí. Độ phân dải điều khiển xác định bởi độ 
phân dải hệ thống điều khiển vị trí và hệ thống phản hồi: là tỷ số của phạm vi 
di chuyển và số bước di chuyển của khớp được địa chỉ hoá trong bộ điều khiển 
của robot: 
Số bước di chuyển = 2". 
với n - số bit của bộ nhớ. 

Ví dụ: Một khớp tịnh tiến của robot có hệ thống điểu khiển 12 bịt di 
chuyển trong phạm vi 100 mm, số bước di chuyển có thể là: 2'? = 4096. Độ 
phân đải tương ứng là: 

xo =0.0244 mm. 
4096 

Độ đi chuyển của robot là tổng các dịch chuyển thành phần. Do đó độ phân 

đải của cả robot là tổng các độ phân dải các từng khớp robot. 
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Độ chính xác cơ khí trong cơ cấu truyền động các khớp và khâu phản hồi 
-_ của hệ thống điều khiển secvo sẽ ảnh hưởng đến độ phân dải. Các yếu tố làm 
giảm độ chính xác có khí như khe hở trong hộp truyền, rò rỉ của hệ thống thuỷ 
lực, tải trọng trên tay robot, tốc độ đi chuyển, điều kiện bảo dưỡng robot,... Độ 
chính xác cơ khí giảm sẽ làm giảm độ phân dải. 


1.3.7. Độ chính xác: Đánh giá độ chính xác vị trí tay robot có thể đạt 
được. Độ chính xác được định nghĩa theo độ phân dải của cơ cấu chấp hành. 
Độ chính xác di chuyển đến vị trí mong muốn sẽ phụ thuộc vào độ dịch chuyển 
nhỏ nhất của khớp. Khi coi cơ cấu cơ khí có độ chính xác rất cao, có thể định 
nghĩa sơ bộ độ chính xác bằng một nửa độ phân dải điều khiển như minh hoạ 
trên hình 1.2. l 

Trong thực tế, độ phân dái bị ảnh hưởng bởi một số yếu tố. Độ chính xác 
sẽ thay đối tuỳ thuộc vào phạm vi di chuyển của tay robot: phạm ví di chuyển 
càng xa bệ robot, độ chính xác càng giảm đo độ mất chính xác cơ khí càng lớn. 
Độ chính xác sẽ được cải thiện nếu di chuyển của robot được giới hạn trong một 
phạm vi cho phép. Tải trọng cũng ảnh hưởng đến độ chính xác, tải trọng lớn sẽ 
gây ra độ chính xác cơ khí thấp và làm giảm độ chính xác di chuyển. Thông 
thường độ chính xác di chuyển của robot công nghiệp đạt 0,025 mm. 


„ đích 
độ chính xác LẮNG: 
điểm được 


địa chỉ hoá| độ phân dái điều khiển 









điểm được 
địa chí hoá 


Hình 4.2. Minh hoạ độ chính xác và độ phân dải điều khiển. 


1.3.8. Độ lặp lại: Độ lặp lại đánh giá độ chính xác khi robot đi chuyển để 
với tới một điểm trong nhiều lần hoạt động (ví đụ 100 lần). Do một số yếu tố 
mà robot không thể với tới cùng một điểm trong nhiều lần hoạt động, mà các 
điểm với của robot nằm trong một vòng tròn với tâm là điểm đích mong muốn. 
Bán kính của đường tròn đó là độ lặp lại. Độ lặp lại là đại lượng có ý nghĩa quan 
trọng hơn độ chính xác. Độ chính xác đánh giá bằng sai số cố định; sai số cố 
định có thể phán đoán được và có thể hiệu chỉnh bằng chương trình. Nhưng sai 
số ngẫu nhiên sẽ khó có thể khử được. Độ lặp lại cần phải được xác định bằng 
kết hợp nhiều thực nghiệm với tải trọng và các hướng di chuyển khác nhau 
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(phương thẳng đứng và phương nằm ngang,..). Độ lập lại của các robot công 
nghiệp thông thường là 0,025 mm. 


1.3.9. Độ nhún: Độ nhún biểu thị sự địch chuyển của điểm cuối cổ tay 
robot đáp ứng lại lực hoặc mômen tác dụng. Độ nhún lớn có nghĩa là tay robot 
dịch chuyển nhiều khi lực tác dụng nhỏ và ngược lại. Độ nhún có ý nghĩa quan 
trọng vì nó làm giảm độ chính xác dịch chuyển khi robot mang tải trọng. Nếu 
tay robot mang tải trọng nặng, trọng lượng tải trọng sẽ làm cho cánh tay robot 
bị dịch chuyển. Khi robot thực hiện Bia công khoan, ấn mũi khoan vào chỉ tiết, 
phản lực sẽ làm có cấu tay di chuyển,... Nếu robot được lập trình trong điều kiện 
không tải của cơ cấu tay, độ chính xác sẽ giảm trong điều kiện làm việc có tải. 


1.4. Hệ thống chuyển động robot 


Hệ thống chuyển động robot công nghiệp đảm bảo cho robot có thể thực 
hiện các nhiệm vụ trong không gian làm việc bao gồm các chuyển động của 
thân, cánh tay, cổ tay giữa các vị trí hoặc chuyển động theo một quỹ đạo đặt 
trước. Hình 1.3 biểu diễn các bộ phận chính của hệ thống chuyển động robot. 
Bô phận cơ bản của robot là cánh tay (am) gồm một số thanh nối cứng (link) 
liên kết nhau bởi các khớp mềm (joint); thân (bệ) (base); cổ tay(wrisU; bàn tay 
(hand) và các ngón tay (fingers). Cánh tay robot được gắn lên thân (bệ); cổ tay 
-_ được gắn ở thanh nối cuối cùng của cánh tay robot; bàn tay (còn gọi là cơ cấu 
tác động cuốt) được gắn lên cổ tay có nhiệm vụ thực hiện các nhiệm vụ theo 
yêu cầu công nghệ: cầm nắm hoặc gia công. Chuyển động robot công nghiệp 
được chia làm hai loại: Chuyển động cổ tay và chuyển động cánh tay. 

1.4.4. Bậc tự do của robot 

Bậc tự do của robot là số toạ độ cần thiết để biểu diễn vị trí và hướng của 
vật thể ở tay robot trong không gian làm việc. Để biểu diễn hoàn chỉnh một đối 
tượng trong không gian cần 6 tham số: 3 toạ độ xác định vị trí đố tượng trong 
không gian và 3 toạ độ biểu diễn hướng của vật thể. Như vậy một robot công 
nghiệp điển hình có số bậc tự do là 6. Số bậc tự do của robot công nghiệp sẽ 
tương ứng với số khớp hoặc số thanh nối của robot. 

Nếu số bậc tự do nhỏ hơn 6, không gian chuyển động của tay robot trong 
sẽ bị hạn chế. Với một robot 3 bậc tự do, tay robot chỉ có thể chuyển động dọc 
theo các trục x, y, z và hướng của tay không được xác định. Tương tự robot có 5 
trực trong mặt phẳng. Ngược lại số bậc tự do lớn hơn 6, sẽ có nhiều lời giải biểu 
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diễn vị trí và hướng của robot trong không gian và sẽ có nhiều phương án điều 
khiển chuyển động. 







Trục 2 
(Shouder 


Cơ cấu 
tịnh tiến 


Mặt bích lắp 
bàn tay 


Hình 1.3. Hình dạng điển hình và các bộ phận của robot công nghiệp. 


1.4.5. Khớp robot 


Khớp là khâu liên kết hai thanh nối có chức năng truyền chuyển động để 
thực hiện di chuyển của robot. Thanh nối gần với thân robot là thanh nối vào, 
thanh nối ra sẽ chuyển động so với thanh nối vào. 

Khớp robot gồm hai loại: Khớp tịnh tiến và khớp quay như minh họa trên 
hình 1.4. Khớp tịnh tiến thực hiện chuyển động tịnh tiến hoặc trượt thanh nối 
đầu ra. Các dạng cơ cấu khớp tịnh tiến là cơ cấu xilanh-piston, cơ cấu kính viễn 
vọng... Khớp quay có ba dạng: R, T, V. Khớp quay đạng R có trục xoay vuông 
góc với trục hai thanh nối. Dạng khớp quay T có trục xoay trùng với trục hai 
thanh nối. Dạng khớp quay V có trục xoay trùng với trục thanh nối vào và 
vuông góc với trục thanh nối ra. 
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Hình 1.4. Các dạng khớp của robot: 
a- Khớp tịnh tiến; b- Khớp quay dạng R; 
c- Khớp quay dạng T; d- Khớp quay dạng V. 


1.4.6. Cổ tay robot 

Cổ tay robot có nhiệm vụ định hướng chính xác bàn tay robot (cơ cấu tác 
động cuối) trong không gian làm việc. Ví dụ bàn tay robot cần định hướng 
chính xác so với chỉ tiết để gắp chỉ tiết. Thông thường cơ cấu cổ tay robot có 3 
bậc tự do tương ứng với 3 chuyển 


động có cấu tạo điển hình như TẾ mot kosoncs 

hình 1.5: Cổ tay xoay xung quanh Cánh tay 
trục thanh nối cuối cùng (Roll), Mặt bích lắp | - 

CỔ tay xoay xung quanh trục nằm S90 9y eo) 

ngang tạo chuyển động lên xuống ÑĐ . 
của bàn tay (Pitch) quay xung % MU KỤ 


quanh trục thắng đứng tạo chuyển Wrist Roll v 
động lắc phải, trái của bàn tay 
(Yaw). 
Hình 1.5. Cơ cấu cổ tay ba bậc tự do. 
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1.4.7. Bàn tay robot (cơ cấu tác động cuối) 

Bàn tay được gắn lên cổ tay robot đảm bảo cho robot thực hiện các nhiệm 
vụ khác nhau trong không gian làm việc. Cơ cấu bàn tay có hai dạng khác nhau 
tùy theo chức năng của robot trong dây chuyển sản xuất: cơ cấu bàn kẹp 
(gripper) và cơ cấu dụng cụ (tool). 

a. Cơ cấu kẹp: Cơ cấu kẹp được sử dụng để cầm giữ một vật thể hoặc chỉ 
tiết ở các robot làm việc trong dây chuyền lắp ráp khi gấp một chỉ tiết và lắp ráp 
một bộ phận của một máy; robot ở dây chuyền đóng gói hoặc ở robot có chức 
năng vận chuyển như gắp một chỉ tiết đặt lên một băng tải hoặc vận chuyển một 
chi tiết từ vị trí này sang vị trí khác... Các chỉ tiết cũng có các loại và hình dạng 
khác nhau: chai, hộp, vật liệu thô hoặc một dụng cụ... 

Cơ cấu kẹp thông thường gồm hai hay nhiều ngón tay. Các ngón tay có 
chức năng biến đổi một dạng năng lượng (điện, cơ khí, khí nén hoặc thủy lực) 
nhờ một cơ cấu chấp hành thành lực để nắm giữ một vật thể. Cơ cấu có khả 
năng mở ra và nắm lại các ngón tay và sinh lực đủ lớn để giữ được một vật thể 
trong tay. Có hai dạng cơ cấu ngón tay để giữ vật thể (hình 1.6). Dạng thứ nhất, 
các ngón tay có hình dạng gần giống như hình dang vật được giữ, như vậy nhờ 
diện tích tiếp xúc giữa các ngón tay mà vật thể được giữ chắc chắn trên tay 
robot. Dạng thứ hai, vật được giữ nhờ lực ma sát giữa ngón tay và chỉ tiết. Trong 
trường hợp này, ngón tay cần phải sinh lực đủ lớn để đảm bảo sinh lực ma sát 
đủ lớn. Để tăng lực ma sát ở các ngón (ay khi giữ vật thể, thông thường các 
ngón tay và các bộ phận gắn trên các ngón tay được chế tạo bằng các vật liệu 
mềm. Đồng thời cũng có tác dụng bảo vệ các ngón tay khỏi bị hư hỏng đo va 
chạm. 





Miếngdạng  Ngón tay 
vật thể 
Ngón tay 
Chỉ tiết 
„_.._. Cơ cấu 
kẹp 
a) b) 


Hình 1.6. Các dạng cơ cấu ngón tay: 


a- Cơ cấu ngón tay có biên dạng như vật thể; 
b- Cơ cấu ngón tay có biên dạng song song. 


1? 


Trong hai dạng cơ cấu ngón tay trên, đạng thứ hai có ưu điểm đơn giản, dễ 
thiết kế chế tạo, tuy nhiên có nhược điểm so với dạng thứ nhất khi cần một lực 
đủ lớn đặt lên vật theo hướng song song với bề mặt ma sát của các ngón tay, 
làm cho vật dễ bị trượt ra khỏi ngón tay. Để chống lại hiện tượng đó, cơ cấu 
kẹp cần được thiết kế để đảm bảo sinh lực cần thiết phụ thuộc vào trọng lượng 
của vật thể, hệ số ma sát giữa vật thể và các ngón tay, hế số gia tốc và giảm tốc 
của vật và hướng của vật thể trong khi di chuyển. Quan hệ lực tác động theo 
hướng song song với bể mặt tiếp xúc và các tham số đó được biểu điễn bằng 
phương trình sau: 

n/F, = w (1-]) 
trong đó: 4 - hệ số ma sát ở bể mặt tiếp xúc giữa ngón tay và vật thể; 
ïì; - SỐ ngón tay; 
E, - lực của cơ cấu kẹp, 
W - trọng lượng của vật thể được kẹp. 

Trong trường hợp lực tác dụng có xu hướng kéo vật thể ra khỏi các ngón 
tay lớn hơn trọng lượng của vật thể, lực cần thiết để giữ vật được xác định theo 
phương trình sau: 

un/F, = wk, (1-2) 
với K, - hệ số hiệu chỉnh tính đến tác dụng kết hợp của trọng lực và lực gia tốc: 
khi lực gia tốc có hướng trùng với trọng lực: ky = 3 và k, = I khi lực gia tốc có 
hướng ngược với hướng trọng lực; nếu lực gia tốc có hướng nằm ngang ku= 2. 





Hinh 1.7. Dạng cơ cấu ngón tay kiểu thanh nối. 
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Một số đạng cơ cấu kẹp khác nhau tùy thuộc vào cơ cấu chấp hành được 
biểu diễn trên hình I.7 - 1.8. Hình 1.7 là một số dạng cơ cấu kẹp kiểu thanh nối. 
ở dạng cơ cấu thanh nối, lực F, được biến đổi thành lực kẹp F, ở các ngón tay. 
Cơ cấu bánh răng — thanh răng được minh họa trên hình 1.8a. Thanh răng dịch 
chuyển tịnh tiến nhờ piston sẽ làm quay cặp bánh răng và các ngón tay sẽ mở 
hoặc đóng lại. Các ngón tay truyền động bởi cơ cấu cam ở hình 1.8b sẽ mở ra 
hoặc đóng lại khi cam đi chuyển theo hướng xác định. Trên hình 1.8&c là cơ cấu 
trục vít - êcu. Trục vít được quay bởi động cơ sẽ dịch chuyển một êcu, từ đó hai 
ngón tay sẽ mở ra hoặc đóng lại. 


Bánh răng Thanh răng 






Hình 1.8. Một số dạng cơ cấu ngón tay: 


a- Cơ cấu bánh rắng - thanh răng; 
b- Cơ cấu cam; 
c- Cơ cấu trục vít - ôcu. 


b. Bàn tay dạng một cơ cấu dụng cụ 


Trong nhiều dây chuyên sản xuất, robot thực hiện nhiệm vụ như một dụng 
cụ để gia công kim loại hoặc một công nghệ đặc biệt như sơn, hàn... Để thực 
hiện các công nghệ đó, dụng cụ có thể được kẹp trên bàn tay robot (cơ cấu kẹp) 
hoặc một dụng cụ được gắn cố định trên cổ tay của robot. Các dạng cụng cụ là: 
Mũi khoan, dụng cụ cắt, đá mài, một bình sơn, cơ cấu hàn điểm, hàn hồ 
quang... Khi bàn tay robot là một dụng cụ, robot cần được điều khiển chuyển 
động của dụng cụ tương tự như điều khiển cơ cấu bàn tay kiểu kẹp. 
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c. Ghép nối (giao điện) giữa robot và bàn tay 

Vấn đề ghép nối và giao diện giữa robot và bàn tay đóng vai trò quan trọng 
thể hiện ở một số chức năng sau: Truyên năng lượng từ các cơ cấu robot cho 
bàn tay; đảm bảo cung cấp các tín hiệu điều khiển cơ cấu bàn tay thông qua tín 
hiệu điều khiển công suất truyền động; Truyền tín hiệu phản hồi từ các cảm 
biến ở bàn tay trở về hệ thống điều khiển. 

Thông thường bàn tay robot được ghép cơ khí với cổ tay bằng một mặt 
bích. Cơ cấu ghép đó cần đảm bảo ba yêu cầu: độ cứng vững, tính mềm dẻo (độ 
nhún) và khả năng bảo vệ quá tải. Độ cứng vững cần thiết để cơ cấu có khả 
năng chịu được các lực liên quan với sự làm việc của cơ cấu tác động cuối. Các 
lực này gồm các thành phần gây ra bởi khối lượng của bàn tay, vật thể ở bàn 
tay; lực gia tốc, lực giảm tốc và các lực tác dụng lên bàn tay robot trong chu kỳ 
làm việc. Yêu cầu độ nhún đảm bảo tính mềm dẻo cho cơ cấu. Ví dụ khi lắp ráp 
một chỉ tiết vào một lỗ của một bộ phận máy với độ kín khít cao (cơ cấu có tâm 
đàn hồi ở xa). Khả năng bảo vệ quá tải nhằm hạn chế và ngăn ngừa sự hư hỏng 
của bàn tay. Có thể dùng các cảm biến để phát hiện sự quá tải: gửi tín hiệu về 
bộ điều khiển nhằm tác động điểu khiển, chỉnh định để ngăn ngừa quá tải. Ví 
dụ, robot nhặt một chỉ tiết nằm trên băng tải đang chuyển động. Nếu tay robot 
._ vướng vào một bộ phận gắn chặt trên băng tải, hệ thống điều khiển phải đáp ứng 
để dừng băng tải và yêu cầu trợ giúp, hoặc robot phải được lập trình sao cho có 
thể chuyển động để gỡ được bàn tay robot ra khỏi vị trí đó. 

Bàn tay robot cần năng lượng để thực hiện nhiệm vụ của dây chuyển công 
nghệ và tín hiệu điều khiển để điều chỉnh sự hoạt động của cơ cấu bàn tay. Các 
phương pháp chủ yếu truyền năng lượng và tín hiệu tới bàn tay là: Khí nén, 
điện, thủy lực và cơ khí, trong đó phương pháp truyền bằng khí nén và điện 
được sử dụng rộng rãi hơn cả. Phương pháp truyền năng lượng tới bàn tay robot 
phải tương thích và phù hợp với hệ thống cơ khí. Ví dụ, sử dụng khí nén để 
truyền động cơ cấu bàn tay sẽ là giải pháp hợp lý khi các khớ¬ robot được 
truyền động bởi các các thiết bị khí nén. Tín hiệu điều khiển sẽ điều khiển cơ 
cấu bàn tay thông qua điều chỉnh sự truyền năng lượng tới bàn tay. 


1.4.6. Các dạng cơ cấu hình học và không gian làm việc của robot 


Cấu hình robot thông thường được định nghĩa theo các khung toạ độ của 
không gian làm việc của tay robot. Có năm dạng cơ cấu hình học điển hình như 
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minh họa trên hình 1.9: Cơ cấu toạ độ đê các, cơ câu toạ độ trụ, cơ câu toạ độ 
cầu, cơ cấu dạng khớp nối và dạng cơ cấu SCARA. 

+ Cơ cấu robot toạ độ Đecac ký hiệu là 3P gồm 3 khớp tịnh tiến chuyển 
động theo phương của các trục của hệ toa độ Đecac. Không gian làm việc của 
bàn tay robot là một hình hộp chữ nhật (hình 1.9a). 

+ Cơ cấu robot toa độ trụ ký hiệu là R2P gồm 2 khớp tịnh tiến của trụ và 
cánh tay và một khớp quay xung quanh trụ robot. Không gian làm việc của bàn 
tay là một hình trụ (hình 1.9b) 





Hinh 1.9. Một số dạng cơ cấu hình học robot: 
a- Cơ cấu Đecac; 
b- Cơ cấu hình trụ; 
c- Cơ cấu hình cầu; 
d- Cơ cấu khớp nối; 
e- Cơ cấu SCARA. 
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+ Cơ cấu robot toạ độ trụ ký hiệu là 2RP gồm 2 khớp quay và 1 khớp tịnh 
tiến. Không gian làm việc của bàn tay là một hình cầu (hình 1.9). 

+ Cơ cấu robot dạng khớp nối ký hiệu là 3R gồm 3 khớp quay tương tự như 
cánh tay người. Đây là một dạng điển hình được sử dụng rộng rãi trong công 
nghiệp (hình 1.9d) 

+ Cơ cấu robot SCARA (Selective Complian Assemply Robot Arm) (hình 
1.9e) có 2 khớp quay có trục song song nhau cho phép tay robot chuyển động 
trong mặt phẳng nằm ngang, một khớp tịnh tiến di chuyển tay theo phương 
thẳng đứng và một khớp quay định hướng bàn tay robot. 


1.5. Hệ thống điều khiển robot 


Liên quan đến đặc điểm làm việc của robot có thể chia bài toán điều khiển 
robot thành hai loại: điều khiển thô và điều khiển tỉnh. Ở bài toán điều khiển 
thô, sẽ xác định luật điều khiển thích hợp để tốc độ, vị trí do đó chuyển động 
của các khớp bám sát quỹ đạo thiết kế trong thời gian quá trình quá độ nhỏ 
nhất. Bài toán thứ hai liên quan đến quá trình khi robot di chuyển tiếp xúc với 
môi trường làm việc như trường hợp của robot lắp ráp một chỉ tiết vào một thiết 
bị máy. Như vậy quá trình làm việc này sẽ yêu cầu điều khiển cả lực và vị trí. 
Phân loại các phương pháp điều khiển robot trình bày ở hình 1.10. 









Điều khiển 
tobot 






Điều khiển thô 
(Điều khiển quỹ đạo) 






Điều khiển tinh 
(Điều khiển lực) 














Điều khiển 
toạ độ khớp 






Điều khiển 
toạ độ Đecac 


Điều khiển 
trở kháng 


Điều khiển 
hỗn hợp 


Hình 4.40. Các phương pháp điều khiển robot. 


Điều khiển chuyển động thô hay điều khiển quỹ đạo có thể thực hiện ở hệ 
toạ độ khớp hay toạ độ đề các tuỳ thuộc quỹ đạo được thiết kế cho toạ độ khớp 
hay toạ độ Đecac. Điều khiển chuyển động tính là điều khiển lực, thực chất là 
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kết hợp điều khiển lực và quỹ đạo, Điều khiến lực gôm điêu khiên trở kháng và 
điều khiển hỗn hợp. 

Sơ đồ khối của hệ thống điều khiển chuyển động (quỹ đạo) robot được vẽ ở 
hình 1.11. Robot gồm n khớp và mỗi khớp sẽ truyền động bởi một động cơ và 
một hệ thống truyền động riêng. Bộ điều khiển vị trí (Bộ ĐKi) có chức năng 
điều khiển chuyển động robot. Khâu Tạo quỹ đạo chuyển động sẽ tính toán các 
quỹ đạo chuyển động mong muốn của từng khớp (q„) từ quỹ đạo mong muốn 
của tay robot (Xu, yạ„ z¿). Quỹ đạo chuyển động mong muốn khớp (q„) là tín 
hiệu đặt vị trí của các bộ điều khiển vị trí của từng khớp. Tín hiệu ra của các bộ 
điều khiển (u,) là tín hiệu điều khiển hệ truyền động của khớp tương ứng. Mỗi 
bộ điều khiển vị trí của từng khớp là bộ điều khiển secvo với tín hiệu phản hồi 
là vị trí của khớp tương ứng được đo bởi các cảm biến vị trí (CBi). Cấu trúc các 
hệ thống điều khiển vị trí và lực robot sẽ được trình bày chỉ tiết ở chương 6 và 7. 


Quỹ đạo đặt khớp Tín hiệu ĐK khớp . x 
(qm) (u,) Vị trí khớp 
(q,) 


(q;) 






Quỹ đạo tay 
(Xa, Y4, Za) 


Hình 4.11. Sơ đồ khối hệ thống điều khiển vị trí. 


1.6. Ứng dụng của robot 


Robot được sử dụng rộng rãi trong nhiều ngành công nghiệp khác nhau. Có 
thể phân loại ứng dụng công nghiệp của robot làm các lĩnh vực chính: Vận 
chuyển, bốc dỡ vật liệu, gia công, lắp ráp thăm dò và các ứng dụng khác. 
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1.6.1. Ứng dụng robot trong vận chuyền, bốc đỡ vật liệu 


Trong ứng dụng vận chuyển, robot có nhiệm vụ di chuyển đối tượng từ vị 
trí này đến vị trí khác. Nhiệm vụ này của robot thực hiện bởi các thao tác nhặt 
và đặt vật thể. Robot nhặt chỉ tiết ở một vị trí và chuyển dời đến một vị trí khác. 
Robot có thể gắp một chỉ tiết ở một vị trí cố định hoặc trên một băng tải đang 
chuyển động và đặt ở một vị trí cố định khác hoặc đặt trên một băng tải khác 
đang chuyển động với định hướng chỉ tiết. Robot có khả năng bốc xếp và vận 
chuyển các chỉ tiết có hình dạng và kích thước khác nhau nhờ các thông tin 
chuẩn vẻ chỉ tiết lưu trữ trong bộ nhớ và robot sử đụng các cảm biến để nhận 
dạng chi tiết thực. Trong dây chuyển sản xuất, robot được sử dụng để đưa chỉ 
tiết và và lấy chi tiết ra khỏi một máy gia công kim loại, máy CNC, máy đột 
đập, máy ép nhựa hoặc dây chuyền đúc. 

Trong công đọan đóng gói, robot có nhiệm vụ xếp các vật liệu (dạng khối 
chữ nhật) lên trên một giá (pallet) và đóng gói; bốc đỡ vật liệu khỏi pallet; xếp 
các sản phẩm vào một hộp caton hoặc nhặt các chỉ tiết ra khỏi hộp. 


1.6.2, Ứng dụng trong lĩnh vực gia công vật liệu 


Trong công nghiệp gia công vật liệu, robot thực hiện nhiệm vụ như một 
máy gia công. Do đó tay robot sẽ gắn một dụng cụ thay cho một cơ cấu kẹp. 
Ứng dụng của robot trong công nghiệp gia công vật liệu bao gồm các công nghệ 
sau: Hàn điểm; hàn hồ quang liên tục; sơn phủ; công nghệ gia công kim loại... 

Hàn điểm là một ứng dụng phổ biến của robot công nghiệp, đặc biệt trong 
công nghiệp lắp ráp ôtô. Hàn điểm có thể thực hiện bằng hai phương pháp: 
Dùng máy hàn điểm và dùng súng hàn điểm. Máy hàn điểm gồm hai điện cực 
ép chặt hai chỉ tiết và cho dòng điện có giá trị lớn chạy qua, kết quả là hai chỉ 
tiết sẽ được hàn dính nhau ở một điểm. Dùng hàn điểm gồm hai điện cực và một 
khung có thể mở hoặc đóng hai điện cực; một cáp lớn dẫn dòng chạy qua. Hệ 
thống súng hàn điểm có trọng lượng và kích thước lớn và gây kuó khăn cho 
người điều khiển trong một dây chuyển sản xuất với tốc độ lớn. Robot sẽ được 
sử dụng rất hiệu quả trong công nghệ hàn điểm này. Trên tay robot sẽ gắn khẩu 
súng hàn điểm và robot sẽ được lập trình để thực hiện trình tự hàn trên sản 
phẩm. Ở day chuyển lắp ráp ôtô, hàng chục robot hàn điểm sẽ làm việc với nhau 
theo một chương trình lặp sắn. Robot hàn điểm phải có kích thước lớn, có khả 
năng mang tải trọng để điều khiển súng hàn có khối lượng lớn một cách chính 
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xác. Robot cần phải đưa súng hàn vào đúng vị trí và đúng hướng ở những vị trí 
người khó thực hiện được. Do đó số bậc tự do robot phải lớn và bộ nhớ máy tính 
phải có dung lượng lớn. Lợi ích của tự động hóa công nghệ hàn điểm sử dụng 
robot là nâng cao chất lượng sản phẩm, thao tác an toàn và điều khiển tốt hơn 
quá trình hàn. 

Hàn hồ quang liên tục sử dụng trong công nghệ hàn đường: ghép hai bộ 
phận kim loại hoặc hàn ống, hàn vành bánh xe... Môi trường làm việc đối với 
người công nhân hàn hồ quang rất nguy hiểm và độc hại: nhiệt độ cao, tia cực 
tím sinh ra trong quá trình hàn sẽ gây nguy hiểm đến thị giác con người... Việc 
ứng dụng robot trong công nghệ hàn hồ quang sẽ cải thiện đáng kể điều kiện 
làm việc của con người, đồng thời nâng cao năng suất và chất lượng sản phẩm. 
Hai loại công nghệ hàn hồ quang sử dụng robot là hàn hồ quang khí — kim loại 
(GMAW hoặc MIG) và hàn hồ quang vonfram khí (GTAW hoặc TIG). Tuy 
nhiên do một số vấn đề về kỹ thuật như nâng cao chất lượng hàn khi có sự thay 
đổi các thành phân của vật liệu hàn và vấn đề kinh tế, nên robot chỉ được sử 
dụng trong công nghệ hàn hồ quang ở các dây chuyền sản xuất có sản lượng 
trung bình và lớn. Hệ thống robot hàn gồm hai bộ phận: robot hàn với que hàn, 
hệ thống cấp dây hàn và bộ phận giữ chỉ tiết hàn có khả năng định vị và định 
hướng chỉ tiết hàn tương đối so với robot. Ví dụ một hệ thống robot hàn như 
hình 1.12. 
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Hình 1.12. Hệ thống robot hàn hồ quang. 
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Sơn phủ bề mặt sử dụng robot đang phổ biến trong công nghiệp thay thế 
cho con người để cải thiện điều kiện làm việc. Một súng phun sơn được gắn trên 
tay robot. Điều khiển súng phun sơn được thực hiện trong quá trình là việc cần 
thỏa mãn các yêu cầu chất lượng sản phẩm. Các tham số được điều khiển là lưu 
lượng sơn phun, áp suất. Ngoài ra độ nhớt, nhiệt độ, cần được duy trì ở mức cho 
phép. Súng phun sơn cũng yêu cầu được làm sạch theo những chu kỳ đặt trước. 
Trong công nghiệp robot được sử dụng để sơn phủ bề mặt ôtô, bề mặt thiết bị 
máy, các thiết bị sinh hoạt... Trong dây chuyển sản xuất, robot sơn là một bộ 
phận của hệ thống, thông thường robot sơn làm việc với hệ thống băng tải mang 
chi tiết cần sơn. Do đó sự làm việc đồng bộ giữa robot sơn và các khâu khác 
trong dây chuyền cũng được đặt ra trong thiết kế tự động hóa dây chuyền sơn. 

Ngoài các ứng dụng cơ bản trên, robot cũng được sử dụng trong các dây 
chuyền gia công kim loại như khoan, mài, đánh bóng, trong các máy cặt tia lửa 
điện, cắt laze... 

1.6.3. Ứng dụng robot trong lắp ráp và kiểm tra sản phẩm 

Công nghệ lắp ráp là lắp một chỉ tiết vào một bộ phận khác. Robot được sử 
dụng trong dây chuyền lắp ráp thông thường ở bốn dạng sau: lắp chỉ tiết vào lỗ, 
lắp lỗ vào chỉ tiết, lắp chỉ tiết nhiều chân vào lỗ và lắp ngăn xếp. Ở công đoạn 
lắp chỉ tiết vào lỗ, robot nhặt một chỉ tiết, thông thường là một chốt, lắp vào một 
bộ phận máy khác. Chi tiết có thể là hình trụ tròn hoặc hình hộp chữ nhật. 
Robot sử dụng trong lắp ráp thông thường có 5-6 bậc tự do để có thể lắp chính 
xác góc vuông của lễ vào góc của lỗ. Lắp ráp lỗ vào chỉ tiết, robot được sử đụng 
lắp các bánh răng vào trục truyền lực... Trong dây chuyển sản xuất bán dẫn, 
robot được sử dụng lắp các phần tử bán dẫn (vi mạch) nhiều chân vào các vị trí 
trên các bảng mạch điện tử. Trong ứng dụng này, robot yêu cầu có số bậc tự do 
cao để có thể định vị và định hướng bất kỳ chỉ tiết trên bảng mạch. Dạng cuối 
cùng là lắp ngăn xếp, robot sẽ lắp chỉ tiết chồng lên chỉ tiết trong một ngăn xếp 
hoặc một rãnh. Ví dụ lắp các tấm thép mỏng của phần ứng động cơ một chiều 
hoặc khung từ máy biến áp. 

Trong công nghiệp lắp ráp, robot có thể hoạt động đơn lẻ để lắp hoàn thiện 
một thiết bị hoặc làm việc trong một dây chuyên, trong đó mỗi robot sẽ có 
nhiệm vụ lắp một chỉ tiết trong một thiết bị máy. 

Robot cũng được sử dụng trong công đoạn thử nghiệm và kiểm tra. Một 
trong những ứng dụng của robot trong lĩnh vực đo và kiểm tra sản phẩm là các 
máy đo toa độ (Coordinate Measurement Machine - CMM). Máy đo toạ độ 
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._ (CMM) được sử dụng rộng rãi đế kiếm tra kích thước, vị trí và hình dạng của 
các chỉ tiết máy hoặc các bộ phận cơ khí. Cấu tạo của CMM bao gồm các bộ 
phận chính như bàn CMM, bộ phận giảm chấn, cơ cấu đầu đò và hệ thống điều 
khiển. Bàn máy và bộ phận giảm chấn được thiết kế đảm bảo độ ổn định của cơ 
cấu nhằm tăng độ chính xác của phép đo. Đầu đò được di chuyển nhờ cơ cấu 
chuyển động điều khiển vị trí khi chạm vào chỉ tiết sẽ dừng lại, hệ thống điều 
khiển sẽ đọc các giá trị toạ độ và ghi vào bộ nhớ. Từ các giá trị đo được, có thể 
xác định được kích thước biên dạng của chỉ tiết nhờ một chương trình phần 
mềm chuyên dụng. 

Trong công nghiệp sản xuất 
ôtô, CMM được sử dụng để đo và 
kiểm tra các bộ phận của ôtô (hình 
1.13). Trong công nghiệp luyện tôi 
thép, CMM với trang bị các cảm 
biến 3D để xác định kích thước sẽ 
được sử dụng đo chính xác trong 
môi trường nhiệt độ cao. Từ đó 
giảm chi phí sản xuất do tiết kiệm 
nguyên vật liệu thừa. Hình 1.13. CMM sử dụng trong kiểm tra 

Hãng Mitutoyo là một hãng kích thước ôtô. 
cung cấp nhiều thiết bị đo lường 
CNC CMM có độ chính xác cao. 
Máy đo CMM của hãng Mitutoyo 
là sản phẩm đo 3 D có độ chính 
xác cao và tốc độ đo nhanh. Trên 
hình 1.14 là hình dạng bên ngoài 
của máy đo CMM kiểu Bright- 
Strato. Với lựa chọn chế độ quét 
của đầu dò và phần mềm SCANM 
cho phép người sử dụng có thể 
quét bể mặt với chế độ liên tục 
nhanh hơn quét từng điểm. Dựa Hình 1.14. Máy đo CMM kiểu Bright-Strato 
vào một chuỗi dữ liệu các điểm đo của hãng Mitotoyo. 
của vật thể sẽ xác định được các đường nét của vật thể như độ tròn, độn vuông 
của bề mặt vật thể. 
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CHƯƠNG 2 
ĐỘNG HỌC VỊ TRÍ ROBOT 


Trong chương 2 sẽ trình bày động học thuận và động học ngược robot. 
Động học thuận cho phép xác định vị trí và hướng của tay robot (hand) khi đã 
biết các biến của khớp (joint. Động học ngược sẽ tính toán các biến khớp tương 
ứng với vị trí và hướng của tay. Sử dụng công cụ ma trận để mô tả đối tượng: vị 
trí, hướng và chuyển động. Tiếp theo sẽ tính toán động học thuận và ngược của 
cấu hình robot điển hình khác nhau: Toạ độ Đecac, trụ và hình cầu. Cuối cùng 
sử dụng phép biểu điễn Denavit — Hartenberg xây dựng phương trình động học 
thuận và ngược của các robot cấu hình khác nhau. 

Trong chương này robot (manipulator) coi là một cơ cấu không có bàn tay 
(end effector). Tuy nhiên các robot công nghiệp có cơ cấu bàn tay với kích 
thước và hình dạng khác nhau gắn vào cổ tay. Khi đó khi tính toán động học 
thuận và ngược, cần phải bổ sung thêm cấu hình của bàn tay robot. 


2.1. Biểu điễn ma trận 


Ma trận được sử dụng để biểu diễn một điểm, một vectơ, một khung toa độ, 
các phép biến đổi tịnh tiến, quay và biểu diễn một đối tượng trong một khung 
toa độ. 


2.1.1. Biểu diễn một vectơ trong không gian 
Một vcctơV biểu diễn điểm P trong không gian được xác định bằng ba 


thành phần là hình chiếu của vectơ trên ba trục của khung toạ độ chuẩn (hình 
2.1). 


V=v,i+v.]+v,k __ Œ-]) 


trong đó: v, v„ v, - các hình chiếu của vectơ Vtrên 3 trục của khung toạ độ 
chuẩn. 


1k - tương ứng là các vectơ đơn vị của các trực X,y,Z. 


Vectơ V cũng có thể được biểu diễn ở đạng ma trận như sau: 
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V=lVy =Ă\, Nộ v„Ƒ (2-2) 


Trong kỹ thuật robot, vectơ V được biểu diễn bằng một ma trận cột với bổ 
sung thêm thành phần thứ tư là hệ số tỷ lệ w như (2.3): 


<I 
II 


(2-3) 


x SH 


wW 
VỚI V„ = XÍW; Vy = YÍW; V„= Z/W; 
wW Sẽ có giá trị 1 hoặc 0; nếu w=l, giá trị các 
thành phần không thay đổi. Khi đó ba thành 
Hình 2.1. Biểu diễn vectơ trong phần x, y, z sẽ biểu diễn một điểm. Nếu w = 0 
kh các thành phần v„, vụ, v, sẽ là œ. Khi đó ba 
thành phần x, y, z sẽ biểu diễn hướng của một vectơ, x, y, z là các thành phần 
của vectơ đơn vị của các trục. Vectơ đơn vị là vectơ có hướng trùng với hướng 
của trục và môđun bằng I. 





2.1.2. Biểu diễn một khung toạ độ 


Một khung toạ độ B có gốc trùng với gốc của khung toạ độ chuẩn A biểu 
diễn bằng 3 vectơ đơn vị iạ, J„,kạ (hình 2.2). Mỗi vectơ đơn vị được biểu diễn 





Hình 2.2. Biểu diễn khung toạ độ có trùng Hình 2.3. Biểu diễn khung toạ độ 
gốc với khung toa độ chuẩn. trong khung toạ độ A. 


bằng 3 thành phần là hình chiếu trên các trục của khung toạ độ chuẩn. Như vậy 
có thể biểu diễn khung toạ độ B ở dạng ma trận: 
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lpy dạy kạ, 
t†) = Ìny Ủny kạy (2-4) 
lạ; Jp; kạ, : 

Nếu khung toạ độ có gốc không trùng với gốc khung toạ độ chuẩn (hình 
2.3), gốc khung toạ độ B biểu diễn tương đối so với khung toạ độ chuẩn bằng 
vectơ P{P,.p,p,]. Khi đồ khung toạ độ B được biểu diễn ở đạng ma trận (4x4), 
trong đó 3 cột đầu biểu điễn hướng của khung toa độ B, cột thứ tư xác định vị trí 
của gốc khung toạ độ B: 

ị Bx Jm k 8 Py 
lg j kạy 
(le| PS sM, THị ví Q-5) 
lụ, Jn; kạẹ, D; 
00 0 1 

Ví dụ 2.1: Khung toa độ B đặt ở vị trí [3,5,7] với vectơ đơn vị lộ Song song 
trục x, vectơ js tạo với trục y một góc 459, 
vectơ kụ tạo với trục z một góc 45” (hình 
2.4). Khi đó khung toạ độ B được biểu diễn 
bằng ma trận:(hình 2.4), Khi đó khung toạ 
độ B được biểu diễn bằng ma trận: 

1 0 0 3 
0 0/707 -0/707 5 
0 0707 0707 7 





tF} = 


Hình 2.4. Ví dụ biểu diễn khung toa 
0 0 0 1 độ trong không không gian. 


2.1.3. Biểu diễn một vật thể rắn 


Để biểu điễn một đối tượng trong không gian, gắn một khung toa độ lên 
đối tượng — “Khung toạ độ đối tượng” và xác định vị trí của khung đối tượng đó 
trong không gian. Đối tượng gắn cố định ở “khung toạ độ đối tượng”, nên có thể 
xác định được vị trí và hướng của đối tượng đó trong “khung toa độ đối tượng”. 
Do đó, khi đã xác định được quan hệ giữa khung toạ độ đối tượng trong khung 
toa độ chuẩn, sẽ xác định được vị trí và hướng của đối tượng so với khung toa 
độ gốc cố định (hình 2.5). Như đã trình bày ở 2.1.2, một khung toạ độ trong 
không gian được biểu điễn bằng một ma trận vuông (4x4), trong đó ba cột đầu 
biểu diễn hướng của ba vectơ đơn vị ba trục và cột thứ tư xác định vị trí của gốc 
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khung toạ độ trong khung toạ độ chuẩn: 


lọtx~ Jom Kex Ppm 


Rui]2 loYy Jety Kpfy Pony » (0:6) 
lb jpz Kpr, Dpr 
0 0 9 1 


Một điểm trong không gian chỉ có ba bậc tự do, tương ứng với ba thành 
phần trên ba trục khung toạ độ chuẩn có thể di chuyển dọc theo ba trục của 
khung toa độ chuẩn. Tuy nhiên một đối tượng có sáu bậc tự do, nghĩa là đối 
tượng có thể đi chuyển dọc theo ba trục x, y, z và quay xung quanh ba trục đó. 
Như vậy cần sáu thông số mô tả vị trí đối tượng trong khung toạ độ chuẩn và 
hướng tương đối so với các trục của khung toạ độ chuẩn sẽ hoàn toàn xác định 
được đối tượng đó trong không gian. 

Nhưng ma trận (2-6) có 12 thành phần: 9 thành phần mô tả hướng và 3 
thành phần xác định vị trí. Do đó, cần phải có sáu phương trình ràng buộc để 
khử 6 thông số và số thông số độc lập là 6. Các điều kiện ràng buộc này là các 
thuộc tính của một khung toạ độ: 

® 3 vectơ đơn vị vuông góc nhau. 

® Modun của vectơ đơn vị bằng ]. 

Hai thuộc tính trên được thể hiện bằng sáu phương trình ràng buộc sau: 


q) 1j=0 

2 IiK=U 

@)  kị=0; 

4 |Ỉ=l @-7) 
6 J[=t 

(6) R ¬ 





Ba phương trình đầu (2-7) có thể Xa 
thay thế bằng tích có hướng sau: Hình 2.8. Ví dụ biểu diễn khung toạ độ 
% ] = K (2-8) trong không không gian. 
Ví dụ 2.2: Xác định các thành phần chưa biết của ma trận biểu diễn một 
khung toa độ sau: 


?j 0925 
0707 2 ? 3 
HP | cọ y2 
0 001 
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Giái: Các giá trị cột thứ tư (5,3,2) không ảnh hưởng đến các phương trình 
ràng buộc. Sử dụng phương trình (2-7): 
1j, + ly + LJ,= 0 <> Í,0 + 0/707. j, + l¿j, = 0; 
1K + ky, + Lk,= 0 © 1.k, + 0/707, K, + ï„0 =0; 
k„J, + kjy + k,J,= 0 © k0 + k,.jy + 0.j,= 0; 


2 +i2+i2=E << i2+0/7072+i2=1; 
3 2+j+j2=! œ@ 0Œ+i2+j2=l 
kˆ+k?+k2=1 @ kÈ+ k//+0= l1; 
Đơn giản các phương trình trên, nhận được: 
0/701. j, + Ì,j,= 0; 
i„k, + 0,707. k, =0; 
ky =0; 
l2+i2=0,5; 
ý ti“ 
k„° +k2= 1, 
Giải 6 phương trình trên được: ì, = + 0,707; ¡, =0; Jy =0; J, =L; k,= + 0,707 
và k, =~0,707. Trong đó ¡, k, phải cùng đấu. Các ma trận kết quả là: 


0/707 0 0707 5 -0/707 0 -0/707 5 
0707 0 -0/707 3 _| 0707 0 -0707 3 
tịÌ= 0 1 0 2 Œ;}= 0 Ị 0 2 
0 0 0 ] 0 0 0 ] 


Bài toán trên có thể giải bằng sử dụng tích có hướng của ¡ và j: 
TIẾT 
ï jJ.Kk ¬. 
i„ lý 1, |=k,i+k,j+kjk 
1, $y J 
®-(/jj,~1jýŸ-((1,=tL)‡(Jy—È=ki+k,j+k,K 
Thay số vào các phương trình trên và cân bằng từng thành phần trên các 
trục, nhận được các phương trình sau: 
0,707], ¬ 1jy= k, 
-,j,=k, 
ijy„=0 
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2.1.4. Ma trận biến đổi đồng nhất 


Ma trận biến đổi đồng nhất là một ma trân vuông (4x4) được sử đụng để biểu 
diễn đồng thời hướng và vị trí của một khung toạ độ có dạng sau: 


Lo Jv K¿ P, 
F}=|b b xì P 
; j¿ K¿ P, 
0 0 0 1 


-. m† Bm- 


@-9) 


2.2. Phép biến đổi 


Phép biến đổi được sử dụng để mô tả sự chuyển động trong không gian của 
một khung toạ độ hoặc một vectơ, một đối tượng di chuyển trong không gian 
tương đối so với khung toa độ chuẩn (khung toạ độ gốc). Có 3 dạng phép biến 
đổi: 

e Phép biến đổi tịnh tiến đơn đọc theo các trục toạ độ. 
* Phép biến dối quay đơn xung quanh các trục toa độ. 
® Phép biến đối kết hợp của phép tịnh tiến và phép biến đổi quay. 


2.2.1. Phép biến đổi tịnh tiến đơn 


Một khung toa độ (có thể bao gồm một đối tượng) di chuyển trong không 
gian nhưng không thay đổi hướng của nó sẽ tương ứng với một phép biến đổi 
tịnh tiến. Khi đó các vectơ đơn vị của khung toạ độ đó không thay đối hướng, 
gốc toạ độ của khung toa độ sẽ di chuyển tương đối so với khung toạ độ gốc 
(hình 2.6).VỊ trí mới của khung toạ độ so với khung toạ độ gốc được xác định 
bằng cộng vectơ dịch chuyển tịnh tiến và 
vectơ biểu diễn vị trí ban đầu của gốc 
khung toạ độ. Do hướng của các vectơ đơn 
vị không thay đổi trong khi dịch chuyển, 
phép biến đổi tịnh tiến được biểu diễn 
bằng ma trận vuông (4x4) như sau: 





1 0 0a 
T = Trans(a,b,c) = h 3 ì : (2-10) X 
0001 Â Hình 2.6. Biểu diễn pnép biến đổi 


tịnh tiến đơn trong không gian. 


Dạng ma trận biểu diễn khung toạ độ mới được xác định bằng nhân ma trận 
biến đổi tịnh tiến với ma trận biểu diễn khung toạ độ ban đầu: 


33 


h 


tF2}= 


c=c=c=-- 
cc=—c 
c—=cc 
—¬œơ»% 


99 0 | 0 0 0 ] 
Phương trình (2-1 1) được biểu điễn ở đạng ngắn gọn: 
Em = Trans(a,b,c)XF ¿ 


Tóm tắt: 


l„ Jí Ký P.| |ix J« kụ Pu +4 
xi J Kx Py|B |ly lv Kựụ pụ +b 
lự. dạ k„ Pa tự lạ K„ Đ„ +€C 


(2-11) 


(2-12) 


® Ma trận biểu điễn vị trí mới của khung toạ độ xác định bằng nhân ma trận 


phép biến đổi với ma trận biểu diễn khung toạ độ ở vị trí ban đầu. 


e Các vectơ hướng của khung toạ độ không thay đổi sau phép biến đổi tịnh tiến 


đơn, vectơ vị trí biểu điễn gốc toạ độ của khung toạ độ mới là Ð + p+. 


Ví dụ 2.3: Khung toạ độ F được dịch chuyển theo trục x là 9 đơn vị, theo trục Z 


là 5 đơn vị. Tìm khung toạ độ mới của khung toạ độ sau phép biến đối. 


0527 -0/574 0/628 5 
0369 0819 0439 3 
- 9,766 0 0,643 8 
0 0 0 l 


F= 


Giải: Sử dụng phương trình (2-1 1) hoặc (2-12) có: 
F„z„ = Trans(a,b,c).F,¿ = Trans(9,0,5).F.„ 


0966 0 0643 8| |-0766 0 
0 0 0 1 0 0 


2.2.2. Phép biến đối quay đơn 


09 0527 -0,524 0628 5 0527 —0574 0,628 l4 
00 0269 0819 0439 3| |0369 0819 0,439 3 
I5 0,643 13 
0 1 


I 


Giả sử khung toạ độ B (ia, j;.k ¿) đặt ở gốc khung toạ độ gốc (x,y,Z) và có 
các trục song song với khung toạ độ gốc. Điểm P (P,,p„;p,) trong khung tọa gốc 
và có các toạ độ ”p,,°p,,°p, trong khung toạ độ B. Khung toạ độ B quay xung 


quanh trục x một góc 0; điểm P sẽ quay cùng khung toạ độ B. Trước khi quay, 
toạ độ của P trong hai khung toạ độ như nhau; sau phép quay, toa độ điểm P 
trong khung toạ độ B giữ nguyên, và toạ độ P trong khung toạ độ gốc sẽ thay 


đổi (xem hình 2.7). 
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Trên hình 2.8 biếu diễn các toạ độ điểm P trong mặt phăng (y, z) trước và 
sau phép quay xung quanh trục x. Toạ độ điểm P trong khung toạ độ gốc sau 
khi quay là Ð,„› Py„› P„„. Từ hình 2.8 nhận được các phương trình sau: 

Sản == P, 


P„„=°P, cosÔ —PP, sin9 (2-13) 


By =°P, sia9 +°P, cosÔ 





Hình 2.7. Toạ độ điểm P trong khưng toạ độ B trước và sau khi quay. 


Phương trình (2-13) viết ở dạng ma 
trận: 
P„|] [1 0 0 PP, 
=|0 cos8 —sinÐ PB (2-14) 
Đựy Q sm8 cos8 |? P, 


Từ (2-14) nhận được ma trận biểu 
điễn phép quay xung quanh trục x như 





Pyn Ÿ (2-15): 
ẩ l0 0 
Hình 2.8. Toạ độ điểm P trong khung toa độ đe 
gốc và khung toạ độ quay nhìn từ trực quay x. Rot (x,8)= ì _ :. (2-15) 


trong đó: C0 ký hiệu của cos9; SØ ký hiệu của sinÐ. Tương tự, ma trận biểu diễn 
phép quay đơn xung quanh trục y và z: 


c0 0 s C6 -S0 0 
Rot(yÐ8)=| 0 I 0| và RotzÐ)=lS06 C6 0 (2-16) 
-§9 0 C0 00 1 
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Phương trình (2-14) có thể viết ở dạng chính tác biểu thị mối quan hệ giữa 
các khung toạ độ khác nhau, bằng cách sử dụng các ký hiệu: “Tạ - phép biến 
đổi biểu diễn khung toạ độ R so với U (universe); FP - biểu điễn điểm P trong 
khung toạ độ R, và Up - điểm P trong khung toạ độ U: 


UP=UT,.RP (2-1?) 
Ví đụ 2.4: Điểm P(2,3.4) trong khung toạ độ quay một góc 90° quanh trục 


x khung toạ độ gốc. Xác định vị trí điểm P sau phép quay so với khung toạ độ 
gốc và vẽ minh họa. 





Hình 2.9. Phép quay khung toạ độ so với khung toạ độ gốc. ì 


Giải: Áp dụng (2-14) có: 


PP 10 0 P., 10 02 2 
P„„|=|0 C9 -S6||°P,| =|0 0 -1||3|=|—4 
P, 0 §8 Œ6 8p 0 1 4 

Hình 2.9 minh họa các khung toạ độ và điểm P trước và sau khi biến đổi. 

2.2.3. Phép biểu diễn kết hợp 

Tổng quát, phép biến đổi có thể gồm một số phép biến đổi tịnh tiến và quay 
so với khung toạ độ cố định hoặc khung toạ độ đang chuyển động. Xét một 
phép biến đổi kết hợp ba phép biến đổi đơn so với khung toạ độ gốc theo thứ tự 
sau: : 

(1) Quay xung quanh trục x một góc œ. 

{2) Tịnh tiến dọc theo các trục lần lượt là a, b, c. 

(3) Quay xung quanh trục y một góc B. 
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Một điểm P ở khung toạ độ B quay và tịnh tiến so với khung toạ độ gốc nên 
toạ độ của P sẽ dịch chuyển. Sau phép quay thứ nhất, toa độ điểm P biểu điễn so 
với khung toạ độ gốc bởi phương trình sau: 

Mại = Rot(x,œ).®°0, (2-18) 
trong đó: t,ạ,- vectơ biểu diễn điểm P sau phép biến đổi thứ nhất so với 
khung toạ độ gốc. Sau phép biến đổi thứ hai, toạ độ điểm P biểu thị bằng vectơ: 


ti;; = Trans(a,b,c)0;, = Trans(a,b,c).Rot(x,œ).”ữ, (2-19) 
Tương tự, sau phép biến đổi thứ ba, toạ độ điểm P được biểu diễn so với 
khung toạ độ chuẩn là: 
0g; = Rot(y,B)ũ;, = Rot(y,B)Trans(a,b,c).Rot(x,œ)”ữ, (2-20) 
Theo (2-8) + (2-20), sau mỗi lần biến đổi, toa độ điểm P nhận được bằng 
nhân phía trước ma trận của phép biến đổi. Như vậy trình tự viết ma trận trước 
vectơ ”ữ, là ngược với trình tự của các phép biến đổi. Phép biến đổi kết hợp sẽ 
được viết ở dạng sau: 
T = Rot(y,B}Trans(a,b,c)Rot(x,œ). 


Ví dụ 2.5: Một điểm P [7,3,2]Ÿ đặt trong khung toạ độ B. Xác địnE toa độ 
điểm P trong khung toa độ gốc khi dịch chuyển khung toa độ B bằng các phép 
biến đổi so với khung toạ độ gốc theo thứ tự sau: 

1. Quay xung quanh trục z một góc 90”. 

2. Quay xung quanh trục y một góc 90”. 

3. Tịnh tiến theo các trục x,y,z tương ứng 4,-3,7 đơn vị. 

Giải 

Vectơ biểu diễn điểm P sau ba phép biến đổi trên là: 


“Up =Trans (4,-3,7).Rot(y,90°).Rot(z,90°).°, 


I0 0 4 0 0 1 0/0 —I 0 0||7 6 
_10 I1 0 -3 0 1 0 01 0 0 01131 |4 
J0 01 7 -100 00 0 1 0|12| |10 

90001 9 0 0 1j0 0 0 1||I ] 


Hình 2.10 minh họa thứ tự các phép biến đổi trên. 
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Hình 2.10. Minh họa phép biến đổi ví dụ 2.5; 
a- Khung toa độ ban đầu; c- Sau phép biển đổi thứ hai; 
b- Sau phép biển đổi thứ nhất;  d- Sau phép biển đổi thứ ba. 


2.2.4. Phép biến đổi so với khung toa độ di chuyển 


Các phép biến đổi trình bày ở các mục 2.2.]+ 2.2.3 được thực hiện so với 
khung toạ độ gốc. Các phép biến đổi cũng có thể được thực hiện tương đối so 
với khung toạ độ di chuyển (khung toạ độ hiện tại). Toạ độ của một điểm gắn 
trong khung toạ độ di chuyển được biểu diễn so với khung toạ độ gốc được tính 
bằng phép nhân các phép biến đổi theo thứ tự ngược (so với trường hợp các 
phép biến đổi thực hiện trong khung gốc toạ độ gốc). 

Ví dụ 2.6: Thực hiện các phép biến đổi như ví dụ 2.5 nhưng thực hiện tương 
đối so với khung toạ độ đi chuyển. Xác định ma trận biểu diễn phép biến đổi 
tổng hợp và toa độ điểm P sau các phép biến đổi trên. 

Giải: Trình tự viết phép biến đổi có thứ tự từ trái qua phải theo thứ tự di 
chuyển, nên phép biến đổi kết hợp biểu diễn bằng ma trận sau: 
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ỨTz; = Rot(z,90°).Rot(y,909)/Trans (4.—3,7) 
0-10 0[0 01 0l1090 4 0 -10 3 
_|1 0000 100l010 -3| |0 01 7 
J0 0 10Ì-1000l001 7| |-1 00-4 
0 0 010 001000 1 0 00 1 

Vectơ biểu điễn điểm P sau phép biến đổi VT; là: 

0 -1 0 3171 [09 
j-..|ÐU- 2Ú 1 +7:li |9 
:9~|<#: 0:0 <4J'2|: |t 
0 0 0 1l 1 
Hình 2.LI minh họa aqu uá trình các phép biến đổi. 
z 





Hinh 2.11. Minh họa phép biển đổi ví dụ 2.6: đ) 
a- Vị trí ban đầu: c- Sau phép biển đổi thứ hai; 
b- Sau phép biển đổi thứ nhất, d- Sau phép biển đổi thứ ba. 
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Ví đ„ 2.7: Một khung toạ độ B được quay xung quanh trục x một góc 90°, 

tiếp theo tịnh tiến theo trục z hiện tại một đoạn 3 đơn vị, tiếp theo quay 
xung quanh trục z của khung toạ độ chuẩn một góc 90°. Cuối cùng tịnh tiến dọc 
theo trục y hiện tại một đoạn 5 đơn vị. 

a. Viết phương trình và tính ma trận biểu diễn phép biến đổi. 

b. Xác định vị trí cuối cùng của điểm P [1,5,4]” gắn ở khung toa độ B. 


Giải: 

a. Trình tự viết phép biến đổi tuân theo quy định sau: Phép biến đổi thực 
hiện so với khung toạ độ gốc được viết ở phía trước (theo thứ tự từ phải qua 
trái), phép biến đổi thực hiện so với khung toạ độ di chuyển (khung toạ độ hiện 
tại) được viết ở phía sau (theo thứ tự từ trái qua phải). Như vậy phép biến đổi 
tổng hợp được viết như sau: 


TT, = Rot(z,90°).Rot(x,90° }Trans(0,0,3).Trans(0,5,0) 


0 -1 0 01 0 0 OÏ1 0 0 0[1 0 0 01 [0013 
_|! 0 0 0j0 0 -1 0J0 10 00 19 5| 1000 
J0 0 1 60Ìo 1 0 0lo o1 3lo o1 0L|010 5 

0 0 0 10 0 0 1o 00 1l 00 1| |0 001 


c. Vị trí điểm P sau các phép biến đổi trên được xác định: 


0 0 1 311 [7 

z z.|J1 00 0l5| |1 
TH Tge ƯC Ễ 

0 1 0 54) I10 

0 0 0 i1|J1) |1 


Hình 2.12 minh họa phép biến đổi trên. 
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Hình 2.12. Minh họa phép biến đổi ví dụ 2.7: 
a- Vị trí ban đầu; 
b- Sau phép biển đổi thứ nhất, 
c- Sau phép biển đổi thứ hai; 
d- Sau phép biển đổi thứ ba; 
e- Sau phép biển đổi thứ tư. 


2.3. Nghịch đảo của ma trận phép biến đổi 


Để đơn giản, sẽ tính nghịch đảo của ma trận biểu diễn phép biến đổi quay 
đơn xung quanh trục x: 


L0 9 
Rot(x8)=[|0 C8 —so 
0 %9 Œœ 
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Trình tự tính nghịch đáo cúa ma trận tiến hành theo các bước sau: 
ø Tính định thức ma trận; 
se Chuyển vị ma trận; 
s Thay thế các phần tử của ma trận chuyển vị bằng ma trận phụ trợ; 
® Chia ma trận cho định thức. 
Định thức ma trận quay được tính theo: 
D= 1(C?0 + S9) +0 = I 
Ma trận chuyển vị có dạng: 


1 0 0 
Rot(xÐ)=|0 Cô s9 
0 -S0 C6 
Ma trận phụ trợ của từng phần tử của ma trận nghịch đảo có đạng sau: 
L0 0 
Rot(&x,8)„=|0 C8 S0 
0 -§0 C0 


tức là: Rot(x,9)m = Rot(x,9)7. 

Đo định thức ma trận bằng 1, nên nhận được ma trận nghịch đảo của phép 
biến đổi quay theo trục x là: 

Rot (x,8 }! = Rot (x,0 )f 

Tương tự, có thể tính nghịch đảo ma trận phép biến đổi quay theo trục y, z. 
Như vậy phương pháp tính nghịch đảo ma trận quay là tính ma trận chuyển vị. 

Mở rộng cho ma trận biến đổi đồng nhất, có thể chia ma trận biến đổi đồng 
nhất thành hai thành phần: ma trận quay (3x3) và vectơ vị trí. Nghịch đảo ma 
trận quay là ma trận chuyển vị. Phần vectơ vị trí (cột thứ tư) có các thành tương 
ứng là - B.n, -B.6, -B.ã. Một cách tổng quát ma trận phép biến đổi có dạng: 


1„ s„ ây Đ„ 
T=Ì|y 9y 3y ÐĐy 
n, 9, 3z P, 
0 0 01 


Ma trận nghịch đảo có dạng: 


n, n, n, -Pñ 
+ _|Oy Oy ©, —p.0 ti 
+ A, ây â, ĐÃ 2 1ẾN 
0 0 ‡ 
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Có thể kiểm nghiệm bằng thực hiện phép tính: T.T” = I. 


Ví dụ 2.8: Tính ma trận biểu diễn phép biến đổi Rot (x, 40° ÿ"L, 
Giác: 


L0 0 0 LÔ 0 0 0 

s |0 0/766 -0/643 0| sa |0 0766 0/643 0 
EOHX.40) |0 0433 076: 0|: RGUU40)7 =|  nuua 0766 0 
00 0 1 00 0 1 


2.4. Khung toa độ trong không gian làm việc của robot 


Một robot làm việc trong phân xưởng có nhiệm vụ di chuyển đến chỉ tiết P 
và khoan một lỗ ở chỉ tiết đó như hình 2.13. Để mô tả vị trí và hướng của các bộ 
phận của robot, chỉ tiết và lễ khoan trong không gian làm việc, đặt 5 khung toạ 
độ, Khung toạ độ U là khung toạ độ gốc thường là khung toa độ phân xưởng: 
khung toạ độ R gắn trên thân (bệ); khung toa độ H gắn trên cổ tay robot, hoặc 
thanh nối cuối cùng của cánh tay robot sẽ biểu diễn vị trí và hướng của cổ tay so 
với thân robot (khung toạ độ R). Vị trí và hướng của đầu mũi khoan, hoặc trong 
trường hợp tổng quát là điểm kẹp của bàn tay robot mô tả bằng khung toạ độ E. 
Vị trí của chỉ tiết xác định bằng khung toạ độ P. Vị trí của lỗ khoan mô tả so với 
khung toạ độ chuẩn U được xác định theo hai đường độc lập nhau: đường qua 
chỉ tiết và đường qua robot. Ma trận biểu điễn khung toạ độ E so với khung toạ 
độ gốc được viết như sau: 

TS = ỨTG ẤT, ST nÚT PT (2-22) 
trong đó: 


 T - phép biến đổi biểu diễn đâu mũi khoan hoặc điểm kẹp sơ với khung 
toạ độ gốc; 


Tạ - phép biến đổi biểu diễn vị trí và hướng của robot trong khung toạ độ 
gốc; 

*T:,- phép biến đổi biểu diễn vị trí và hướng của tay robot so với thân TObot; 

#Tg- phép biến đổi biểu diễn vị trí và hướng của điểm kẹp so với tay robot; 


T, - phép biến đổi biểu diễn vị trí và hướng của chỉ tiết trong khung toa độ 
ĐỐC; 
°T; - phép biến đổi biểu diễn điểm kẹp trên chỉ tiết. 
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Biểu thức (2-22) cho thây rằng, vị trí của điểm E có thể nhận được bằng di 
chuyển từ U tới P, và từ P đến E; hoặc bằng dịch chuyển từ U đến R, và từ R 
đến H và từ H đến E. 





Hình 2.13. Các khung toạ độ trong không gian làm việc của robot. 


Trong thực tế, các phép biến đổi ”Tạ, "Tạ, ĐT,, *T; đã biết. Vị trí của robot 
trong khung toạ độ U- trong phân xưởng sẽ được đặt trước; ngay cả với robot di 
động, vị trí của robot ở mọi thời điểm cũng có thể xác định được vì bộ điều 
khiển luôn phải bám theo vị trí của thân robot ở mọi thời điểm. Phép biến đổi 
#Tẹ biểu diễn điểm cuối của dụng cụ so với tay robot được xác định trước vì cấu 
hình và kích thước của đụng cụ gắn lên bàn tay robot đã biết. Vị trí của chỉ tiết 
P trong không gian làm việc sẽ biết trước nhờ các thiết bị nhận dạng ảnh 
(camera) hoặc các cảm biến khác, do đó sẽ xác định được ma trận biểu điễn 
phép biến đổi ”T;. Đồng thời ma trận PT; cũng được xác định trước vì cần phải 
xác định trước vị trí cần khoan trên chi tiết (thông thường được thiết kế trước 
thông qua các bản vẽ). Như vậy duy nhất phép biến đổi biểu điển tay robot so 
với thân robot *T;, chưa biết cần xác định nhằm điều khiển tay robot để có thể 
khoan được một lễ trên lỗ chỉ tiết P. Phép biến đổi *T;, có thể xác định từ 
phương trình quan hệ (2-22) bằng nhân trước và sau các ma trận nghịch đảo 
tương ứng: 


 T,=( 7T} ŸTT.@Tp} (2-23) 


Phép biến đổi *T;, sẽ được sử dụng để xác định các biến của khớp tay robot 
(góc quay hoặc độ dịch chuyển) cần thiết đảm bảo tay robot có thể khoan một 
lỗ trên chỉ tiết P. 


Ví dụ 2.9: Trên một robot sáu bậc tự do, đặt một camera ở thanh nối 5 để 
quan sát một đối tượng và xác định khung toạ độ đối tượng so với khung toạ độ 
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của camera. Tính toán chuyển động cần thiết của tay robot để có thế gắp được 
đốt tượng đó. Biết các phép biến đổi sau: 


0 0 -13 0-100 
s _J0 -1 0 0Ì. s„ |! 0 0 01. 
Team “| r0 0 sỈ TH 00.541 
0 0 01 0 0 01 
0012 L000 
CAM _J]JI!I 00 2|, mm 10100 
Tor =Ìp go 4 Te“Ìo 013 
0001 0001 


Giải: Sử dụng phương trình (2-22), có thể viết phương trình mô tả quan hệ 
các khung toa độ trong không gian làm việc của robot: 
®T; ”T¡, Tạ,  Tpr = #T¿ Tcau _ 


Do ŸT; xuất hiện cả hai vế của phương trình nên có thể ước lượng. Ma trận 
 Tz„ chưa biết được xác định từ các ma trận khác đã biết. Do đó: 


 Tpr = ( TE }!. Ấn Y Tca-° #Tưy 


!L00 00 ¡10 0J0 0 -!13J0 912 
_|J0 10 01-100 00 -I 0 010902 
1001-30 01 -4Ì-1 0 0 5Ì0104 
000 1/000 10 0 0 10001 
-1 0 0 -2 
0 10 1 

VY:  Ter=Ío ọ - sa 
0 0 0 1 


2.5. Động học thuận và ngược của các cấu hình 
robot điển hình 
Giả sử một robot có cấu hình đã biết: độ dài các thanh nối và góc quay của 
các khớp hoặc độ dịch chuyển các khớp tịnh tiến. Bài toán động học thuận là 


tính toán vị trí và hướng của tay robot tương ứng với cấu hình robot xác định. 
Ngược lại, động học ngược sẽ tính toán các góc quay của'các khớp hoặc độ dịch 
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chuyển của khớp tịnh tiến tương ứng với vị trí và hướng của tay robot. Nghĩa là, 
sẽ tính toán các giá trị biến khớp cần thiết để đặt tay robot ở vị trí và hướng 
mong muốn. Động học ngược đóng vai trò quan trọng cho điều khiển tay robot 
theo vị trí và hướng đã đặt trước. 

Đối với bài toán động học thuận, sẽ xây dựng hệ phương trình mô tả động 
học cấu hình robot và sử dụng hệ phương trình này sẽ tính toán được vị trí và 
hướng của tay robot tương ứng với các biến khớp đã cho. 

Để biểu diễn vị trí và hướng của một đối tượng trong không gian, gắn một 
khung toa độ lên đối tượng và mô tả vị trí và hướng khung toa độ đó trong 
khung toa độ gốc. Cần 6 tham số hoặc 6 bậc tự do để mô tả vị trí và hướng của 
vật thể. Tương tự đối với robot, để xác định vị trí và hướng tay robot, gắn một 
khung toạ độ lên tay robot và tìm phép biến đổi biểu diễn khung toạ độ tay 
trong khung toa độ gốc. Từ đó sẽ xây 
dựng được hệ phương trình động học z "n 
thuận biểu điễn quan hệ giữa vị trí và 
hướng của tay so với các biến khớp. 
Hình 2.14 minh hoa các khung toạ độ 
tay, khung toạ độ gốc. Phần nối giữa 
2 khung toạ độ phụ thuộc vào cấu 
hình robot. 

Trong mục này sẽ phân tích riêng 
biệt các phương trình mô tả vị trí và 
phương trình mô tả hướng, sau đó sẽ X 


kết hợp các phương trình đó thành Hình 2.14. Khung toạ độ tay trong khung 
một hệ phương trình. (SE IS: 


cCỊ 


ĐỊ 


2.1.5. Phương trình động học vị trí thuận và ngược biểu diên vị trí 


Vị trí gốc khung toạ độ gắn lên vật thể có ba bậc tự đo, do đó có thể sử 
dụng ba thông tin để xác định vị trí của vật thể đó. Bốn cấu hình *:ến hình của 
robot là: 

- Toa độ ĐÐecac; 

- Toạ độ hình trụ; 

- Toạ độ hình cầu; 
- 'Toạ đệ khớp nối. 
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Ì!. Cấu hình toạ độ Đecac: 


Ở robot cấu hình toa độ Đecac, 
tay robot có thể chuyển động tịnh tiến 
theo ba trục x, y, z (hình 2.15). Tất cả 
các cơ cấu chấp hành là cơ cấu tịnh 
tiến; ví dụ Robot IM7565, và cơ cấu 
robot toạ độ Đecac không có chuyển 
động quay, ma trận biểu diễn chuyển 
động của điểm P là ma trận biến đổi 
tịnh tiến. Do đó ma trận biến đổi biểu 





thị phương trình động học thuận của Hình 2.15. Cơ cấu toạ độ Đecac. 
VỊ trí tay robot trong hệ toạ độ ĐÐecac có dạng: 
L00P, 
Tụ„„.=” Tụ = : h ` ợ (2-24) 
000 Ũ 
Ví dụ 2.10: 


Tính toán các chuyển động trong toạ độ Đecac cần thiết để vị trí mong 
muốn của gốc khung toa độ trong robot Đecac ở điểm P{3,4,7]. 

Giải: 

Sử đụng (2-24), phương trình động học thuận mô tả ẦT,, với điểm P là: 


— ——- 
— =ä1 + (Q2 


L0 
L0 py| |0 ï 
01 p,| |00 
00 900 
2. Cơ cấu toạ độ trụ 


Cơ cấu toạ độ trụ gồm hai chuyển 
động tịnh tiến và một chuyển động 
quay theo trình tự: tịnh tiến một đoạn r 
theo trục x, quay một góc œ xung 
quanh trục z và tịnh tiến một đoạn l„ 
theo trục z (hình 2.16). Tất cả các phép 
biển đổi thực hiện so với khung toạ độ 
chuẩn, do đó phép biến đổi biểu điễn 





Hinh 2.16, Cơ cấu toạ độ trụ. 
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gốc của khung toạ độ tay trong khung toạ độ chuẩn được tính như sau: 


#T,= Tr„= Trans(0,0,1). Rot(, œ}. Trans (r,0,0) (2-25a) 

I 9 0 0ÍCœ -¬-Sœ 0 0Ï1 0 0 r Ca -Sœ 0 rCœ 

90 1 0 0|Sơ Cơ 0 0Ì0 1 00 Sơ Cơ 0 rSœ 
ÑT.= = 2-25b 
#100110 0 1óol001E9 0" 6 1:1 K00) 

9 0 0 IÍ0 0 0 1Ì0 0 0 0 00 1 


Ba cột đầu của ma trận (2-25b) biểu điễn hướng của khung toạ độ tay 
robot. Tuy nhiên, trong mục này chỉ quan tâm đến vị trí gốc của khung toạ độ 
tay, biểu diễn bằng cột thứ tư. Khi đó khung toạ độ sẽ có các trục song song với 
các trục của khung toạ độ chuẩn và vị trí gốc khung toạ độ được xác định bằng 
cột thứ tư. Khung toạ độ đó nhận được bằng quay xung quanh trục a một góc -œ 
được biếu diễn bằng ma trận: 


Cơ -§œ 0 rCœllC(-cœ) — S-œ) 0 0 I0 0 rCœ 
RT, - sœ Cơ 0 r§œ|5(-o) C(-ø) 0 0 : 9 l 0 rSs (2-25c) 
0 01 1 0 0 I 0Ị J0 0 1 1, 
0 0 0 ï 0 0 0 1 900 1 


Ví dụ 2.11: Tính toán các biến khớp của robot cơ cấu trụ để đặt vị trí của 
khung toa độ tay ở vị trí [3,4,7]? 

Giai: Sử dụng (2-25b) các thành phần của vectơ vị trí của khung toạ độ tay 
được tính như sau: 

l,= 7; rCœ = 3; rSœ = 4; 

Suy ra tgœ = 4/3 và œ = 53,]9: 

Từ đó tính được r = 5; 

Vậy l,=7;r=5;ơ=53,1° 

4. Cơ cấu toạ độ cầu 

Cơ cấu toạ độ cầu gồm một chuyển động tịnh tiến và hai chuyên động quay 
theo trình tự: tịnh tiến theo trục z một đoạn r, quay xung quanh trục y một góc B 
Và quay xung quanh trục z một góc y (hình 2.17). Các phép biến đổi thực hiện 


so với khung toa độ gốc. Do đó ma trận biểu diễn phép biến đổi kết hợp có 
dạng: 


Tụ = Rot(Z„). Rot(y,B).Trans(0,0,r) (2-26a) 
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~Cœ CB 
Sœ 
0 
0 


©=—cCCc 
m= © CC 
©©c CC 

2) 
sc8e# 
— C© C©GC 
CC — 
CC c 
“—— 
—¬ Cc¬ 


0 
-§B 
0 


CPCy -5Sy SBCy rSBCy 
T.~ CBSy Cy SB%y rSB.Sy 
sát 0 0 Cổ rCB 

0 0 0 I 


(2-26b) 


Tương tự, như cơ cấu hình trụ, ba 
cột đầu của ma trận (2-26b) mô tả 
hướng của khung toạ độ tay; cột thứ tư 
biểu diễn vị trí của gốc khung toa độ. 
Như vậy ma trận biểu diễn vị trí của 





khung toa độ tay sẽ có dạng: 
L0 0 rSBCy 
_I0 1 0 rSB8% 
T=Í0 @ { rCB KG 
9000 l 


Hình 2.17. Cơ cấu toạ độ cầu. 


Ví dụ 2.12: Xác định các biến khớp để có thể đặt gốc của tay robot cầu ở vị 
trí [3,4,7]F. 
Giải: Áp dụng ma trận (2-26c) có: 
r.SB.Cy = 3; 
r.SB.Sy = 4; 
ICB =7. 
Từ phương trình thứ ba, thấy rằng CP> 0 sử dụng phương trình đầu nhận 
được: 
tgy=4/3 
Nếu SB > 0 có: y = 53,1”; Sy = 0,8; Cy = 0,6; r.SB = 3/ 0,6 = 5; r.CB = 7 
=8=35,5. 
Nếu SB < 0 có: y = 233,1” Sy = - 0,8; Cy = - 0,6; r.SB = - 3/0,6 = - 5; 
r.C =7—=B=-35,5° và r = 8,6. 
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2.5.2. Phương trình động học thuận và ngược biêu diên hướng 
Ì. Góc Roll, Pụch, Yaw (RPY) 


Định hướng tay robot bằng ba phép quay xung quanh các trục hiện tại 
ä,o,1ï của khung toạ độ định hướng tay robot. Nếu giả thiết, khung toạ độ hiện 
tại của tay robot song song với khung toạ độ chuẩn, hướng của khung toa độ tay 
robot và khung toạ độ chuẩn sẽ như nhau trước khi tay robot chuyển động. Nếu 
ban đầu khung toạ độ tay robot không song song với khung toạ độ chuẩn, định 
hướng cuối cùng của tay robot sẽ là kết hợp phép biến đổi định hướng ban đầu 
với phép biến đổi định hướng RPY bằng nhân phía sau phép biến đổi ban đầu 
với phép biến đổi RPY. 


Trong trường hợp định hướng tay robot, tay robot đã được đặt ở vị trí cố 
định, phép biến đổi quay RPY sẽ được biểu diễn tương đối so với khung toạ độ 
hiện tại của tay robot. 





Hình 2.18. Phép biển đổi quay RPY. 


Phép biến đổi quay RPY được mô tả trên hình 2.18 gồm các phép quay sau: 
s Quay xung quanh trục ä (trục z của khung toạ độ tay robot) một góc ó - 
phép RolỎ. 


® Quay xung quanh trục ö (trục y của khung toa độ tay robot) một góc 8 - 
phép Pirch. 


. ® Quay xung quanh trục ñ (trục x của khung toạ độ tay robto) một góc \/ - 
phép Yaw. 


-Ma trận biểu diễn phép biến đổi RPY có dạng: 
RPYQ,9,W) = Rot(z,0).Rot(y,0). Rot(x,w) (2-27a) 
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Cj.CÔ C¿.50.Sự - S¿Cự Cj.S0Cự + Sÿ.Sự 
_! S4C6 Sj.S0 Sự + Có.Cụ S¿.S0.Cụ — Cộ.Sự 
RPY(4,6.)=| _ sọ C9.Sự C6.Cự 
0 0 0 
Ma trận biểu diễn vị trí và hướng của tay robot trong hệ toạ độ gốc sẽ là 
tích của ma trận biểu diễn phép tịnh tiến và quay RPY: 


L0 0P, 


010 
= v[ ,Ô, 
by#. Tp. RPY(¿,9,ự) 


900 t 


(2-27b) 


—Ẳc=Cc=c= 


Hội Hệ 


Để giải bài toán động lực học ngược cho phép biến đổi quay RPY, nhân hai 
vế của phương trình (2-27a) với [Rot(z,$)}': 


[Rot(z.ð)]'.RPY(4.0,)= Rot(y,8). Rot(x,w) (2-28) 
Ma trận biểu diễn hướng của tay robot được viết ở dạng tổng quát: 


n, öo, 4â, 0 
RPY($,6,)= ny o, ay, 0 
n, o0, a, 0 

0 0 01 


Khi đó (2-28) có dạng: 


f¡() fÍ¡() f;Œ@) 0 C8 S0Sự S0Cự 0 
f;() f;@) f;(a) 0 k 0 Cự -_Sự 0 (2-29) 
f;(@) f¿() f;(a) 0| |—-S9 C6Sự Cô6Cụ 0 

0 0 0 1 0 0 0 l 


trong đó; f,,( V) = V„.Cụ + V,.Sộ; 
f¡( V) = V..Có - V..Só; 
t( V) = V„ 
vectơ V có thể là a,o,n. 
Cân bằng hai vế của phương trình (2-29), sử dụng khái niệm hàm atan2, 
nhận được các phương trình sau: 
f;(ñ) =0 <> nựCó — n,.S$ =0 <> $ = atan2(n,, n,) 


và ÿ‡=atan2(n,-n,) (2-30a) 
f;(ñ)=-S œ> n,=-—S0 


fu(nñ)=C8 <> n,.Cộ + n.Sẻ = CÔ 
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do đó: Ø = atan2( — n„, (n,.Cả + n..S )) (2-30b) 
Cự =f;(ð)= oy.Cộ — o,.S¿ 
Sự =—f;( 4) =- (ay.C@ - a, Sẻ ) 

do đó:  = atan2 ((a,.Sẻ — ay.Cả), (oy Có — o,.Sẻ )) (2-30c) 


Ví dụ 2.13: Xác định phép biến đổi RPY để phép biến đổi biểu diễn vị trí 
và hướng của tay robot có dạng: 


", O0, 4, P, 0354 -—0,674 0,649 4.33 

Rr „|1, 9y 4y py| | 0505 0/722 0/475 2/50 
"Ín, o, a, p,| |-02788 0160 0595 $ 
0 0 0 1 0 0 0 Ị 


Sử dụng các công thức (2-30a), (2-30b), (2-30c) có: 
b= atan2(n,, n,) = atan2((0,505), (0,354)) = 55°: 
8 =atan2( - n„, (n,.Cộ + n..S$ )) = atan2((0,78§), (0,616)) = 52"; 
\ = atan2 ((a,.Sộ - a,.Cð),(o,.Có - o,.Sð)) = atan2((0,259),(0,966)) = 15"; 


P.= 4.33; p,= 2,50; p,= 8. 
Như vậy có thể viết: 


! 00 4433 
R An. 0 1 0 2,50 “ a a 
Tị = 001 §8 .RPY(5°,52°/15°) 
000 1 


2. Pháp Euler 

Phép Euler gồm ba phép quay đơn theo ba trục (hình 2. 19): 

® Quay xung quanh trục ä (trục z của khung toạ độ tay robot) một góc ÿ. 

® Quay xung quanh trục ð (trục y của khung toạ độ tay robot) một góc 9. 

« Quay xung quanh trục ä (trục z của khung toa độ tay robot) một góc tự. 

Khi đó phép biến đổi Euler được viết như sau: 

EULER($,9,u)= Rot(z,$).Rot(y,8).Rot(z) 

Cọ.C0.Cự - S¿Sự — C¿,C0.Sự S0 Cự Có 0 

S¿.C0.Cự + CjSự -—S¿.C0.Sự+ Có Cự S¿.S0 0 
-§0.Cự S0.Sự C6 0 

0 0 0 ] 


(2-31) 
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Hình 2.19, Phép biển đổi Euler 


Tương tự, ma trận biểu diễn khung toạ độ tay robot trong trường hợp tổng 
quát là tích của phép biểu điễn vị trí và phép biểu diễn hướng Euler: 
I0 0P, 
0 LỌP 
001p 
000 1 

Để giải bài toán ngược, nhân hai vế phương trình (2-31a) với ma trận 
{Rot(z,j)]', nhận được phương trình sau: 


KT, = > |Euler(0.8,) (2-32b) 


+ 


n, ö, a, 0Ô 
Rotz.g|D» °x 4x ĐÍ~ Rot(y,0).Rotz,w) (2-33a) 
n, o0, a, 0 
§-‹Ø..8.1 
f¿@) f„Œ@) f,@) 01 [C8Cự -C8S$ Sð 0 
: fa() f„() f¿Œ) 0 Sự Cụ 0 0 sử 
ẶC: 3 5 = -33b 
HUẸC tạ) f2) fŒ) 0| |-S0Cự sô có 0|} ) 
0... 0-70 1 0 0 0] 


trong đó:  f,( V)= V,.Có + V,.Sẻ 
f;;( V) =- V,.S¿ + V..Có 
f(V)=V, 
vectơ V có thể là a,ð,n. 
Giải phương trình (2-33b) nhận được: 

= atan2(ay, a,) 

tự = atan2 ((-n,.Sẻ + n..Có), ( - o,.Sẻ + o,.Có )) (2-34) 

Ø =atan2((a,.Có + ay.SỐ ), a,) 


Ví dụ 2.14: Xác định phép biến đổi Euler để tay robot được xác định bằng 
phép biến đổi sau: 


h, ƠØ, a, DP, 0579 —~0,548 —0/604 0 
R†.- Hy Oy ây Dy - 0540 0813 -—-0220 0 
“on, o, a, p„ị |06ll -0199 0766 0 


Ố. ⁄04 2Ð: 1|*| 0 0 0 1 
Áp dụng (2-34) có: 
ÿ= atan2(a,, a,) = atan2((-0,220), (-0,604)) = 200°; 
\ự = atan2((-n,.Sẻ + n„.Có), (-o,.Só + o„.CÓ)) = atan2(0,31; 0,952) = I§°: 
9 =atan2((a,.Cộ + ay.Sð ); a, ) = atan2(- 0,643; 0,766) = - 40", 
2.5.3. Phương trình động học thuận, ngược biểu diễn vị trí và hướng 


Ở các Tục trên đã trình bày các phép biểu diễn hướng, vị trí của một khung 
toa độ. Một cách tổng quát ma trận biểu điễn hướng và vị trí của một khung toạ 
độ so với khung toạ độ chuẩn là tích của hai ma trận: ma trận biểu diễn vị trí và 
ma trận biểu diễn hướng: 

T;, = [ma trận tịnh tiến][ma trận định hướng] (2-35) 

Các phép biến đổi vị trí (ma trận tịnh tiến) và các phép biến đối biểu diễn 
hướng (ma trận định hướng) một khung toạ độ được trình bày tóm tất trong 


bảng 2-1. 
[Me tiận nh ến | Phương tình | Mavindibhvag | Thương nh ] 
SG cap. [ri 
25) ÏTRWQam |7 

Hi  |@26) EULER(¿,0,) (2-31Ð) 


2.6. Động học thuận robot 


Bảng 2.1 












2.6.1. Tham số của thanh nối và khớp 

Xét hai khớp ¡ và ¡+l1, thanh ¡ nối giữa hai khớp ¡ và ¡+1 (hình 2,20). 

Như minh họa trên hình 2.10, a, là độ dài pháp tuyến chung của trục khớp ¡ 
và i+l; œ là góc giữa hai trục của khớp í và i+l (góc giữa trục i+l và đường 
thẳng song song trục ¡ nằm trong mặt phẳng chứa trục i+Ivà trực giao với pháp 
tuyến chung a,). 
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Tương tự xét trục khớp ¡-1. Pháp tuyến chung của trục khớp ¡ và ¡-I là a,„. 

Khoảng cách giữa hai chân pháp tuyến chung của trục ¡ là d,. Góc 9, là góc 
giữa hai pháp tuyến chung của trục khớp ¡. 

Đối với khớp quay, 9, là góc quay của khớp. Do đó đặt Ô, là biến của khớp 
quay. Đối với khớp tịnh tiến, d, là độ địch chuyển tịnh tiến cuả khớp, nên đặt đ, 
là biến của khớp tịnh tiến. 


2.6.2. Phương pháp thiết kế khung toạ độ - phép biểu diễn Danevit- 
Hartenberg 


Để nghiên cứu mối quan hệ giữa các thanh nối, khớp và tay robot, ta đặt 
các khung toạ độ cho các thanh nối. Theo phương pháp biểu diễn Danevit- 
Hartenberg (D-H), khung toạ độ thanh nối ¡ được xây dựng theo nguyên tắc sau 
(hình 2.20): 

+ Gốc khung toạ độ thanh ¡ đặt trùng với chân pháp tuyến chung của trực ¡ 
và i+l và nằm trên trục khớp ¡+1 

+ Trục z; đặt theo phương của trục khớp ¡+]. 

+ Trục x¡ đặt theo phương pháp tuyến chung của trục ¡ và ¡+1 theo hướng 
đi từ trục ¡ đến ¡+]. 

Một số trường hợp đác biệt: 

+ Khi hai trục z cắt nhau: sẽ không có pháp tuyến chung giữa hai khớp. Khi 
đó điểm gốc của khung toa độ là giao điểm của hai trục và trục x được đặt dọc 
theo đường vuông góc với mặt phẳng chứa hai trục z đó. 

+ Hai trục song song, sẽ có nhiều pháp tuyến chung. Khi đó sẽ chọn được 

pháp tuyến chung trùng với pháp tuyến chung của khớp trước. Gốc khung 
toa độ chọn sao cho đ, là nhỏ nhất. 

+ Đối với khớp tịnh tiến: khoảng cách đ, là biến khớp. Hướng của trục khớp 
trùng với hướng đi chuyển của khớp. Hướng của trục được xác định, nhưng vị 
trí trong không gian không được xác định. Khi đó chiều dài a; không có ý nghĩa 
nên đặt a, = 0. Gốc toạ độ đặt trùng với gốc thanh nối tiếp theo. 

Theo nguyên tắc đặt khung toạ độ như trên, bất đầu gắn khung toa độ từ bệ 
(thân) robot là khung toạ độ Ô: trục z„ trùng với trục khớp I. Gốc của khung toạ 
độ thanh 6 đặt trùng với khung toạ độ thanh nối 5. 
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Lê 
đại 
Xi 
8) 
#mỉ Ÿụ Ẩm Zn 2m1 Ÿ 5 
Ân+L 
Xuai Xuai Xnan 
c) P) E) 


Hình 2.20. Thiết kế khung toa đệ thanh nối 


2.6.3. Quan hệ giữa hai khung toạ độ ¡ và i-I 

Một cách tổng quát, quan hệ giữa hai khung toạ độ ¡ và i-1 được xác định 
bằng các phép biến đổi theo thứ tự sau: 

+ Quay xung quanh trục z„, một góc 9, sao cho trực X,¡ trùng với phương 


của trục X,. 
+ Tịnh tiến dọc theo trục z,, một đoạn đ, để gốc khung toa độ mới trùng với 


chân pháp truyến chung trục ¡ và i+]. 
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+ Tịnh tiến dọc theo trục x,., (phương pháp tuyến chung) một đoạn â:. 

+ Quay xung quanh trục x,., một góc ở, sao cho trục z,, trùng với trục Z, 

Các phép biến đổi trên được thực hiện so với khung toạ độ hiện tại (khung 
toạ độ ngay trước đó). Do đó phép biến đổi tổng hợp được xác định như sau: 


A¡ = RotŒ,9,)Trans(0,0,d,)Trans(a,,0,0)Rot(x,œ;). (2-36) 


Thay các ma trận của các phép biến đổi đơn vào (2-36), sau một số biến 
đổi, nhận được ma trận biểu diễn quan hệ giữa hai khung toa độ ¡ và i-1 như 
sau: 


c8, -—s0,cơœ, sØ,sơ, a,c0, 
s8, cÔ,cœ, —cÐ,sơ, a,sÐ, 
A,= (2-37) 
0 SƠ, cơ, đ; 
0 0 0 1 


Ví dụ 2.15: Xây dựng phương trình động học thuận sử dụng phương pháp 
biểu diễn D-H cho robot có cấu hình như hình 2.21. 


* 


x 





Hình 2.21. Cấu hình và các khung toạ độ của thanh nối 
robot dạng khớp nối 6 bậc tự do. 
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Giá thiết các khớp 2,3,4 cùng năm trong mặt phăng, như vậy d, = 0. Hình 
2.21 mô tả các khung toạ độ đặt cho các khớp 1+ 6. Xuất phát từ khớp I, trục z, 
được đặt trùng với trục khớp 1, trục x„, đặt song song với trục x của hệ toa độ 
gốc. Trục (Z„ x„) mô tả gốc của robot và cố định khi robot làm việc tức là khi 
khớp 1 quay. Đối với khớp 2, trục z¡ đặt trùng với trục khớp 2, do hai trục Z„„ Zạ 
cắt nhau nên trục x, sẽ đặt vuông góc với hai trục z„ và z¡. Bằng phương pháp 
đặt khung toạ độ theo phép biểu diễn D — H, trục x; được đặt dọc theo đường 
pháp tuyến chung của trục z¡ và z;, trục x; trùng với pháp tuyến chung của hai 
trục Z¿ Và Z;: trục X, có phương theo đường pháp tuyến chung của trục z¿, z¿. Hai 
trục z,. Z¿ cắt nhau, nên trục x; đặt theo phương vuông góc với hai trục z„, z‹. 
Trực z¿ đặt song song Z4, trục Z¿ biểu diễn chuyển động của khớp 6, còn trục 7 
biểu điễn chuyển động của tay robot (end effector). 

Áp dụng nguyên tắc biến đổi toa độ, lập được bảng D — H như bảng 2.2. 


Bảng 2.2. Bảng D ~ H của robot 


Khung toa độ 





Từ bảng D - H xác định được các ma trận thành phần biểu điễn quan hệ 
giữa hai khung toa độ của 2 khớp ¡, i-1: PA, 


CŒ 0 5S, 0 C¿ -5, 0 a,C, 
: S, 0 -C, 0 S, C, 0 a,§, 
Ai= A,=l 7 SP 

0 1 00 0 0 1 0 

00 0 1 0 0 0 1 

Cy —8, 0 a,C, C¿ 0 -—S, a,C, 
2 S C; 0 A5: 3 mỸ Š 0 C, am; 
Điện 0 0 1 0 Su 0 -1 ] 0 

0 0 0 1 0 0 0 Ị 
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C, -S, 00 
C0 5; Ú 
vở -|lầy0 cổ 0 s. Je Cu 009 
hô i0? Ất" sứ, lô lP 
0 0 10 
0 0 0 1 
0 0 01 


- 


Phép biến đổi biểu diễn tay robot so với thân robot nhận được dạng sau: 


TH = 9A LA 2A 2A, *As2A¿ 


C(CzzC;C¿TSz;z5¿) C¡CCCặa¿CuC¿—S¿z„C¿) C¡Ca¿S; C¡(C¿;jj.a,+ 


=8jS/£, +S,S;5, 8/8, -\ s6 ac# Co äa) 

3i(Cz¿CC¿ 85252) S,(CCz,C,C, =S¿;¿C„) SiCaŠ; 5j(Cjï¡(a¿ + | 

+€¡§;5, —C¡8;S, -{Ị¡C; +€¡;¡a; +C:.4;)! 
SaaC,C¿ + Cas¿5¿) —8;;;C;C¿ +C;¡jC¿) S;yŠ; Sa ca + 


+ổ;¿a; +S;.a¿ 


9 0 9 Ì 


2.7. Động học ngược robot 


Ở bài toán động học thuận, vị trí và hướng của tay được xác định từ các 
biến khớp (góc quay ở khớp quay hoặc độ dịch chuyển ở khớp tịnh tiến) đã biết. 

Để điều khiển robot đi chuyển theo các vị trí mong muốn của tay trong 
không gian, cần xác định các giá trị biến khớp tương ứng với vị trí và hướng của 
tay robot mong muốn. Đây là nội dung của bài toán động học ngược. Bài toán 
động học ngược thông thường khó giải và không có lời giải tổng quát cho mọi 
robot. Để minh hoạ cho phương pháp giải bài toán động học ngược, mục này sẽ 
trình bày động học ngược của robot có cấu hình như hình 2.21. 

Trong ví dụ 2.15 mục 2.6 đã xây dựng phương trình động học thuận của 
robot có dạng: 


ny 9, ây Đy 
Hy Oy 3y Pyẹ 
T, =A,(9,)A;(6;)A;(d)A,(6,)A;(6,)A, (6) =| TÓC ẳ (2-38) 
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Ma trận T; đã biết, tức là vị trí và hướng của khung toạ độ tay robot đã biết, 
cần xác định giá trị các biến khớp. Nhân hai vế phương trình (2-38) ma trận 
nghịch đảo của ma trận A,: (A,}! nhận được phương trình sau: 


(AUJTT: = A2A‡A¿A:A, = !'T, 


(2-39) 


Các ma trận A, (i=l+6) đã được tính ở ví dụ 2.15 mục 2.6. Thay các ma 
trận thành phần vào (2-39), nhạn được phương trình sau: 


C, §, 0 0Ïn, 
0 1 0n, 
5, =CŒ, 0 0n, 
0 0 0 I0 
[f(n fụ(o) fụ,(a) 
Ì tín) f;(o) f;(a) 
fy(n) f;(o) f;(a) 
0 0 9 


trong đó: Í¡¡(V) = C.X+S¡.V 
f¡;(V) =Z 
f;(v) = S,,x- Cụ.V 


Ô; 4. P, 
TP ='T, (2-40a) 
°, 2, P, 

0 1 
f(p) 
f 
¡a(P) =1, (2-40b) 
f;(p) 

1 

(2-40c) 


Với x, y, z tương ứng là các thành phần của vectơ v. 


Ví: fy(n) = Cụn +5,.n, 


Ma trận 'T, được tính theo (2-39) sử dụng các ma trận Aj=2+@) ở ví dụ 


2.15 có đạng: 
KĂNMG: — Su, 
SáaC CC, + ¡Sự 
-5,C, S5 
0 0 


—€,„C,C¿ ~§;C¿ C,,.5, 
—8,,C;€, +C;¡,S, 


€C;;.a, + C¡a; +C¿a, 


S5; - SauA, tŠ¿yg; + 5,4, 
° 0 
0 1 


(2-4) 


Cân bằng các thành phần cột 4 của hai ma trận phương trình (2-40b) với sử 
dụng (2-41) nhận được các phương trình sau: 


f1) = Cua + Cạa; + C;a; 
fiz(P) = Š;:¡4¡ + Su;ay + Sóa; 


f;;(p) =0 
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(2-42a) 
(2-42b) 
(2-42c) 


Từ (2-40), (2-42) ta có: 


S¡.p,- C¡.py = 0 (2-43a) 
Suy ra: 9, =a tan 2(,.P,) (2-43b) 
Sử dụng (2-42a) và (2-42b) kết hợp với (2-40c): 
C¡.x+S¡.ÿy = Cuya¿+ Cai + Ca, (2-44a) 
Đ¿= S;â¿ + S;:a; + S;a¿ (2-44h) 
Viết lại hai phương trình (2-44a) và (2-44b) như sau: 
C,.x+8,.ÿ - Cu¿a¿<= C;ay+ Cca¿ (2-45a) 
D¿- S;u¿4¿= S;yyAy+ Sa; (2-45b) 


Bình phương hai vế của các phương trình (2-45) và cộng lại, nhận được 
phương trình sau: 


(Cịp, +S¡py ~ Ca¿a,)” +(p, —S„¿a,)” =a7+a) +2a,a,(S2S +C¿C,,) 
IEx 1Fy 23494 z 134 rễ Ầ ĐÀ 3 Fe; 


(2-46) 
Sử dụng hàm lượng giác, phương trình (2-46) được viết gọn lại như sau: 
3,5;y + C;C;; =cos[(Ð;+Ð;)— Ø; ]= cos0, 2-47) 
Do đó nhận được: 
Cịp, +S¡p, —C;„a„)° +(p, —S¿„a,)° =a? -a} 
C, V* 1P; IĐy a48„) +ÍP, T—5;;,2,) 2 3 (2-48) 
h 2a;â; 
Từ đó xác định được góc Ð; theo công thức: 
9; = atan2(S.C;) (2-49a) 
trong đó: §; =+/l—C? (2-49b) 


Do trục 2,3,4 song song nhau nên tiếp tục nhân ma trận nghịch đảo của 
Ai— A¿ sẽ nhận được phương trình sau: 
(Aj" (A)? (A¿)” (AT: =A;Á; = T, (2-50) 
Sử dụng các ma trận thành phần A, - A, ở ví dụ 2.15, phương trình (2.50) 
có dạng: 


6l 


ft ¡1 Cz;„f.¡(o) €C,f¡(a) €;;„f¡¡(p)+S:¿,f›(p) 
+Š„jt(n) +S;yfñ;(O) +S;„f;(4)  —C¡ua;T=C,a;~a, 


-f:(n) —f;(o} -ft\;(a) 0 


—8„u„f) —5„,f,(0) —S;;,f,(a) —5„¿fi¡(p)+C;;,ñ;(p) 


+; „f;(n) +; f,(O) +C;,f;(3) +S¡,a; +S¿a, 
0 0 0 I 


CC, -C,S,  S, 
S,C, -S,§, —C, 
S IS 0 
0 0 0 
Cân bằng phần tử (3,3) của hai ma trận và sử dụng (2-40) ta có: 

-B;a„(C¡a, + Siay) + C;u¿a, = Ö 
Từ (2-52) giải ra được 6¿.: 
Ô;;¿ = atan2(a, (C¡a,+ Siay) 


— CC ©© C 


(2-51) 


(2-52) 


(2-53) 


Sử dụng (2-53), xác định được Š„;, và C;;, và sẽ xác định được 9, theo 


(2-49). 
Sử dụng các phương trình (2-44) để tính Ð; như sau: 
Cụ.x+S,.y = Caya¿+ C;aay + C;a; 
Đz= Š;¡:8¿ + S;sáy + S;2; 
Vì: C = CC; — S25¿ và S;; = SC: + CS; nên (2-54) có dạng: 
C¡.x+Si.y = Cua, + (CC; — S;S:)ay + Cạa; 
Pz= S;sa¿+ (S;C; + C5¡)aa + S;a; 
Từ (2-55) giất ra được C; và S;: 
n (C;a; +a;)(Ð, —5;;,4„)—§;a;(p,C + ĐyŠ¡ TC j4) 


S 
° (C;a;+a,)” +Sia) 


C;ị= 
- (C;a; +a,)” +S3a) 


Từ (2-56), xác định được góc 9;. 
8; = atan2(S;,C;) 
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(Ca; +a;)(p,C¡ +p,5¡ — C;a„)+Š;4;(P, —5;;¿a,) 


(2-54a) 
(2-54b) 


(2-55a) 
(@-55b) 


(2-56 


2-57) 


Sử dụng (2-49), (2-57) và (2-53) tính được góc 6: 
Đ,= Ô;¿ - Ö; -Ð, (2-58) 
Cân bằng các phần tử (1,3) và (2,3) của hai ma trận hai vế phương trình 
(2-51), và sử dụng (2-40c) ta có các phương trình sau: 


5s = Cạ¿¿(C¡a, + Siay) tổ, (2-59a) 

€; = -C¡a, + Giay (2-509b) 
Từ (2-59) giải được: 

9; = atan2(S.,Cc) (2-60) 


Để giải góc 9, nhân ma trận nghịch đảo của A; cả hai vế phương trình 
(2-51), nhận được phương trình sau: 


C;[C;;,f (n0) +52; f(0)]} C;[C¿;„ñ,(o)+S„„fñ;(o)] 0 0 
—S;f,;(m) —Š;ñ;(o) 


—8+;zf,(n)+ C;;;f;(n) —5a„l,(0)+C¿;„f;(o) 0 0 


0 0 l 
(2-61) 

0 0 0 1 

R S, 


ứ 
e 
® 
c 

© —=—c©=C 

— ©C C©C 


Từ (2-61) giải được góc Ô,, 
Ø, = atan2({—=S„x;f,(n)+ €,s¡f;(n)]L{—Sa¿jf,,(o)+ CC. f;(0)]) (2-62) 
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Bài tập 


2.1. Ma trận sau biểu diễn một khung toạ độ. Hãy xác định các phần tử chưa 
biết của ma trận đó: 


? 0 -1$5 
_|J? 0 0 3 
sở 7-1 9 2 
0 0 0 1 


2.2. Một hệ toạ độ B có gốc và các trục trùng với khung toạ độ gốc A. Điểm P năm 
trong hệ toạ độ B được biểu điễn bằng vectơ ”B = 5.3.4]. Xác định ma trận 
biểu diễn khung toạ độ B ở vị trí mới và vị trí điểm P so với khung toạ độ A: 
*p. Biết rằng khung toạ độ mới này nhận được bằng các phép biến đổi khung 
toa độ B so với khung toạ độ gốc theo thứ tự: 

s Quay xung quanh trục x một góc 90". 
s Tịnh tiến theo các trục x, y, z tương ứng 5, 3, 6 đơn vị. 
s Quay xung quanh trục z một góc 90°. 
2.3. Một đối tượng được mô tả bằng bốn điểm trong khung toạ độ gốc: 
A[I,0,0, HÏ; 
B[ 0,0,0, 1Ï: 
C[0O,0, 1, 1]; 
DỊ 0, 1,0, HT. 
Xác định ma trận biểu diễn phép biến đổi theo thứ tự sau so với khung toạ độ 
gốc. 
s Quay xung quanh trục ÿ một góc 909. 
® Quay xung quanh trục z một góc 90”. 
® Tịnh tiến theo các trục x 10 đơn vị. 
Xác định dối tượng ở vị trí mới sau phép biến đổi trên. 
2.4. Xác định ma trận nghịch đảo của các ma trận sau: 


0527 —0,574 0,628 2 092 0 0439 5 

T-= 0369 0819 0439 5[ T7 0 I 0 6 
1 |—0,766 0 0643 3Ƒ ? |-039 0 092 2 
0 0 0 1 00 0 1 


2.5. Xác định các góc RPY biểu điễn một khung toạ độ tay robot có dạng: 
0527 -0,524 0,628 4 
T= 0369 0/819 0/439 ó6[ 
_|-0.766 0 0,643 9ƒƑ 
0 0 9 1 
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2.6. Xác định các góc EULER biểu diễn hướng của khung toạ độ tay robot mô tả 
bằng ma trận sau: 
0527 —-0,574 0,628 4 
T- 0369 0819 0439 6[ 
—0,766 0 0,643 9ƒƑ 
0 0 0 1 
2.7. Một robot ba bậc tự do có cấu hình như hình 2. 22. 
(a) Xây dựng bảng tham số D- H. 
(b) Viết các ma trận À,. 
(c) Xác định ma trận T biểu diễn điểm cuối cùng của thanh nối 2. 





Hình 2.22. Cấu hình robot 3 bậc tự do. 
2.8. Một robot bốn bậc tự đo như hình 2.23 
{a) Thiết kế các khung toạ độ cho các thanh nối. 
(b) Xây dựng bảng tham số D-H. 
(c) Viết các ma trận A,. 
(đ) Xác định ma trận T biểu diễn tay robot trong khung toạ độ chuẩn. 





Hình 2.23. Cấu hinh robot 4 bậc tự do. 
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PHỤ LỤC 


PL2.1. Động học vị trí của robot Stanford 


PL2.1.1. Động học thuận của robot Stanford 


Robot Stanford là robot có 6 bậc tự do, gồm 6 khớp: 5 khớp quay và I khớp 
tịnh tiến. Tay robot gồm 3 khớp: 2 khớp quay (khớp 1 và 2) và 1 khớp tịnh tiến 
(khớp 3). Cổ tay gồm 3 khớp dạng cầu (khớp 4,5,6). Hình PL⁄2.! minh hoạ cấu 
hình và các tham số hình học của robot Stanford. 





Hình PL2.1. Cấu hình và khung toạ độ robot Stanford. 
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Áp dụng phép biểu diễn Denavit-Hartenberg, đặt các khung toa độ cho các 
thanh nối của robot Stanford như hình PL2.1. Trên cơ sở các khung toạ độ đã 
thiết kế, lập được bảng Denavit-Hartenberg như bảng PL2.1. 


Bảng PL2.1. 

















cl 0 -—sL 0 c2 0 s2 0 1000 

IM. 1 0 2 0 -c2 0 0 100 
AÁi= h ° : A;= : : - 

0 -I 0 0 0 I 0 d, 0 01 đ, 

0 0 0 I1 0 0 0 Í 000 1 

c4 0 ~s4 0 cs 0 s§ 0 cố —s6 0 0 

s4 0 c4 0 s5 0Ø -c§ 0 s6 c6 0 0 
AÀ,= ;Áy= ¬.. 

0 -Il 0 09 0 1 0 0 00 10 

90 0 01 0 0 0 1 0 0 01 


Nhân ma trận A,...A,„ nhận được ma trận biểu diễn vị trí và hướng của tay 
robot Stanford: 


1, 9, â, D; 
#Tụ =9AIlA 2A SA $A;SA¿= [Di 9 ®y Py 
1, Đ, â„ P, 
0 0 0 1 


trong đó: n, = C,{C;(C,C;Œ, - S,S) - 325;€¿] - S(S,CsC¿ + C;S¿); 
ny = S;[C¿(C,C;Cs - S,5;) - SS,C,] - C(S,C;C§ + CạS,); 
nụ = -5;(C¿C;C¿- 5,54) - ©S,Có; 
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o, = C¡[-C;(C¿C;C¿ + S,C¿) + S;5,S,] - S.(-S,C¿C¿+ C¡Ca); 
oy = S;2[-C(CaCsS; + SCá) + S25;S¿] + C¡S,C¿Cs + CạCc); 
o,= S;(C,C¿S, + S,C¿) + CS¿S; 
a,= €(CŒC,S; + S;C¿) - S55; 
ay = S¡(CCuS; + S;Cs) + CiSu5z; 
a,= - SCạS; + C;C;; 
p, = Ci5;d; - Sid;; 
Py = S¡53;d; + Cịd;; 
P,= Cds. 
PL2.1.2. Động học ngược của robot Stanford 


Trong mục PL2.I.I đã xây đựng phương trình động học thuận của robot 
Stanford có dạng: 
H, ©, â, P, 
n, 0, 4a, p 
T, =A,(6,)A;(9;)A;(đ;)A,(6,)A,(6,)A,(9,)= ` „ _ n (PL2-1) 
0 0 0 1 
Ma trận Tạ đã biết, tức là vị trí và hướng của khung toạ độ tay robot đã biết, 
cần xác định giá trị các biến khớp. Nhân hai vế phương trình (PL2-1) ma trận 
nghịch đảo của ma trận A;: (A,)' nhận được phương trình sau: 
(AT = A;A;A¿A¿Á¿ = Tạ, (PL2-2) 
Các ma trận A; ä=l+6) đã được tính ở ví dụ 2.16 mục 2.6. Thay các ma 
trận thành phần vào (PL2-2), nhận được phương trình sau: 


CC 5; 0 O0lÏn, 0, â„ D, 
-]l ÔÖ 
(cm: TC | [Mp Hy Bột Đi ba (L2-3a) 
—5;¡ C¡ 0D 0n, 0, 4, Pẹạ 
0 0 0 IJ|0 0 0 1 
ft) f,(o) f¡(a) f¡() 
f;(n) f;(o) f;(a) f;(p) —ÙT (PL2-3b) 


f,(n) f;(0) f¿(a) f;(p) Ỷ : 
0 0 0 1 
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trong đó: f,,(v) =C,.x+S..V 
f,;(v)= -z (PL2-3c) 
f¡:(v)= -S,.X+C,.V 
Với x,y,z tương ứng là các thành phần của vectơ v. 
Ví dụ: f (n)=C,.n.+S,.n, 
Ma trận 'T; được tính theo (PL2-2) sử dụng các ma trận Aj( = 2 + 6) ở 
PL2.1.1 có dạng: 


Ca(CẠC¿C-S,5%@}) ~—C¿(CẠCzCa+S4S) C¿C4Ss+ Sạd 
—Š25s5%6 +S;5s5s +S;C; 


S2(C4CsCa—Sa5ø)  ~§;(CaC;C¿+§¿§;) SaC¿§;_  Cạđd; (PL2-4) 


+C;Sz5%, —C;§s%¿ —C;Cs 
S„C;Cạ +CaS 28C v06 S„S; d; 
0 0 0 1 


Cân bằng các thành phần cột của hai ma trận phương trình (PL2-3b) với sử 
dụng (PL2-4) nhận được các phương trình sau: 


f,,(p) = S;.d; l 
f¡;(p) = -Cd; (PL2-5) 
Í;;(p) = d; 

Sử dụng (PL2-3), (PL2-5) được viết ở đạng: 
C¡.p.+S,.py = S;.dà (PL2-6a) 
~p, = -Cd; (PL2-6b) 
-9,p,+C,.py = đ; (PL2-6c) 


Sử dụng (PI.2-6c) có thể giải được 9, bằng cách đặt biến phụ r, $: 


r=-lp‡+ py và =alan2(p,.p,); 


Khi đó p, = r.cỷ và p„ = r.s$. Thay vào (2-43c) nhận được: 
sin($-9,) = đ;/t với mr >d;j/r > 0; a >(49-9,) > 0; 


2 
cos(ÿ ~Ø,)=+ -(⁄) 


Từ đó nhận được các biểu thức sau: 
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(Ò -8)=e| da -(%] (PL2-7) 


2 
8, =atan2(p,,p,)—a |4 /rẻtj (%4 | (PL2-8) 


Sử dụng (PL2-6a) và (PL2-6b) giải được 9; và d,: 
Với d; # 0, từ (PL2-5a) và (PL2-6b) nhận được: 
tgÖ; = (cosÐ,.p, + sin9,.py)/p, (PL2-9) 
Do đó: Ö; = atan2((cosÐ,.p, + sin9,.p,) p,). (PL2-10) 
Tương tự, nhân hai vế phương trình (PL2-1) với ma trận nghịch đảo của A 
(Az'), nhận được phương trình sau: 
(A2 {AJTT, = AaA,A;A¿= *Ts (PL2-1I) 
Thay các ma trận thành phần vào (PL2-1 L), nhận được phương trình sau: 
G;:§;y. 0 01 €. 8 D0 
0 0 I1 0J 0 0 -I 
-§; C;¿ 0 0Ì-%, €; 0 
0 0 9 iÍ 0 0 0 
Thực hiện (PL2-12) nhận được: 
f,(n) f¿(o) f,() f„(p) 
f„(n) f;(o) f;(a) f;(p)| ¿ 


f„(n) f;(o) f,(a) f(p) | 
0 0 9 1 


2 


© 


9x Ay Dx 
Y 9y 3y Py 2T. (PL2-12) 
1 °, â¿ Đ„ 


n 
n 
hự 

0 0 01 


CC C 


T, (PL2-13a) 


trong đó: 
f,,(v) = G(C¡.K+S,.y) - S;Z; 
f;2(V) = -S.X+CjYy) (PL2-13b) 
f;;(v) = S;(C,.x+S,.y) + C;z. 
Với x,y,z tương ứng là các thành phần của vectơ v. 
Ma trận ?T, được trình bày bằng nhân các ma trận A.... A„: 


(CạC;CŒ; ~5484) (CaC:C; +95aŠa) CaSs 6 


?T,=| S4CạCạ-Cạ%,  S4C¿C-C¿§%  S,§; 0Í @L214 
55s SsŠg C;  đ; 
0 0 0 1 
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Sử dụng (PL2-13a) và (PL2-14), cân bằng phần tử (3,4) của 2 ma trận, xác 
định được biến d:: 
dị = f;;(p)=5;(C¡p,+S¡py)+C¿p, (PL2-15) 
Để xác định ð„, Ð„, nhân lần lượt ma trận nghịch đảo của A; và của A, vào 
hai vế của phương trình (PL2-1 1), ta nhận được: 
(A'(A2'(A2} (AT, = A;Á, = 4T, (PL2-16) 
Thay các ma trận thành phần vào phương trình (PL2-16), nhận dược 
phương trình ma trận dạng tổng quát: 
fun) f„(o)} fạ(a) f„(p) CC, -Cj§, S, 
f,(n) f¿(o) fa(a) f„„(p) € S,C¿ -—C,§, -C, 
f;(n) f¿(o) fs(a) f;(p) Š, C, 0 
0 0 0 1 0 0 0 


(PL2-17) 


m=-= 


trong đó: f,¡(v) =Ca[C;(C,.x+S,.y)-S;z]+S,(-S,.x+C,.y); 
f;z(v)= -S;(C¡x+S,y); (PL2-18) 
f;;(v)= -5S,[C¿(C,.x+S;.y)-S;z]+C,(-S,.x+C,.y); : 
Cân bằng phần tử (3,3) của hai ma trận của (PL2-18), nhân được phương 
trình quan hệ Ô„ với các thông số đã biết: 


¬ 4l C¿(C¡.a,+S,.a,)-5;a,]+C¿(-S¡.a,+C¡.a,) =0 (PL2-19) 
Từ (PL2-19) xác định được 9, theo công thức sau: 

B, = atan2[(-S¡.a,+C..a,),ÍC;(C,.a,+S,.a,)-S;a,]) (PL2-20a) 
hoặc: Ô,= atan2[-(-5;.a,+C.a,),-[Cạ(C¡.a,+S.a,)-S;a,}) (PL2-20b) 


Kiểm tra giá trị 8, bằng sử dụng các phần tử (1,3) và (2,3) của ma trạn phương 
trình (PL2-14): 
C¿§, = Ca(C¡.a,+S..a,)-S,a, (PL2-21a) 
S¿8 = -Sua,+C,.a, (PL2-21b) 
Từ (PL2-21) thấy rằng nếu 8, > 0, ð¿ được xác định theo (PL2-20a), nếu 0, 
<0, 9; xác định theo (PL2-20b). Nếu 6,= 0, có thể lựa chọn 6, bất kỳ. 
Góc 0; có thể xác định bằng sử dựng các phần tử (1,3) và (2,3) của các ma 
trận phương trình (PL2-17): 
5; = Cu[C2(C,.a,+5,.a,)-5;a,]+§,(-S..a,+C, .ây) (PL2-22a) 
C; = 5;(C¡.a,+S,.a,)-C¿a, (PL2-22b) 
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Và nhận được: ; = atan2(S„,C,) (PL2-23) 
Nhân A¿' vào hai vế phương trình (PL2-16) và sử dụng các biểu thức ma 
trận thành phần, nhận được phương trình chứa biến Ö„ cần xác định: 


f(n) f;(o) f;,(a) đụ (œ) C, -—5, 0 0 
f,(n) f„(o) f,(a) f„¿(p) z S%, CC, 0 (PL2-24) 
,;(n) £, (o) Í;;(a) £Q, (p) 0 0 1 0 
0 0 0 1 0 0 01 
trong đó: 
fs(v) =Cs{Cạ[C2(C¡.X+S,.y}-S¿Z]+S,(c5,.x+C¡.y)}+5:[-S;(C¡x+S,y)-C;z]; 
fg(v)= -S,C¿(C,X+Šiy)-S2Z]+Ca(-5ix+Cyy); (PL2-25) 


f2„(v}= ~5; (C;ÍC(C,.x+S¡.y)¬S;z]+S,(-S,.x+C,.y)]+S;{-Cs(C¡x+S,y)+C;2]. 


Sử dụng các phần tử (1,2) và (2,2) của các phương trình ma trận (PL2-24), 
được: 


§, =- Cz[C4[C;(C,.o,+S,.o,)-S;o,†+S/(-5,.o,+C,.oy]+S/I-S;(C,o,+S,6,)-C;o,]; 
Cạ= -S,[C;(C¡o,+S,o,)-S;o,]+C4(-S,o,+C(o,) (PL2-26) 
Góc 9; được xác định bằng sử dụng (PL2-26): 
8; = atan2(S,,C2) (PL2-27) 
'PL2.2. Động học robot SCARA 
PL2.2.1. Động học thuận của Robot SCARA 


Robot SCARA (Selective Compliance Assembly Robot Arm) (hình 
PL2.2a) có cấu trúc đặc biệt thích hợp với lắp ráp. Robot SCARA có 4 bậc tự do 
với 3 khớp quay (2 khớp ở cánh tay và 1 khớp quay cổ tay) và một khớp tịnh 
tiến. Trục của các khớp quay 1,2,4 và khớp tịnh tiến 3 song song nhau. ` 


đ; 


ú, 





Hình PL2.2. Cấu hình và các khung toạ độ của robot SCARA. 


T2 


Các khung toạ độ các thanh nối của robot SCARA được thiết kế như hình 
PL2.2b và các tham số theo biểu diễn D-H được cho ở bảng PL2.2 


Bảng PL2.1 





Sử dụng ma trận (2-37) tính được các ma trận A, như sau: 


C5200 Œ; =8: 9 a;Œ; 
SỐ C 0 aS, S, C 0 ajS, 
AI= $ A;= z 
0 0 1d 6.":d0: 31-6 
0 0 0 1 9 0 0 1 
100 0 cc Sẽ, 00 
0 10 0 Š, Œ, 0. 9 
A;= : A,= 
0 01 -d, §‹ vÕ^ JẾ cấn 
0 00 1 0 00 1 


Nhân ma trận A,...A„ nhận được ma trận biểu diễn vị trí và hướng của tay 
robot SCARA 


ny 9/ 4, P, Cụ, —§;, 0 aCj+a,C,; 
ny 0, a, p Sa Cụ, 0 a/S,+a¿S, 
NT SA AjAyAv= | 9 Pp 2YÌz 
194.4): IÊ. ni 0 §. 1/48,<0.-d) 
0 0 1 0 0 0 l 
(PL2-28) 


PL2.2.2. Động học ngược của Robot SCARA 


Giải bài toán động học ngược robot SCARA dựa trên phương trình động 
học thuận (PL2-28): 
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Cụ,=n, (PL2-29a) 


Sịz¿= Tý (PL2-29b) 

a¡Ci + tạC:; = P, (PL2-29c) 

điŠt + A512 = Py (PL2-29d) 

dị - đị — dị = p, (PL2-29e) 
Sử dụng (PL2-29c) và (PL2-29d): 

a;Ch; = p, - aiCi (PL2-30a) 

8aŠiy = Ðy - AiSi (PL.2-30b) 


Bình phương hai vế hai phương trình (PL2-30) và cộng lại, nhận được 
phương trình sau: 


8) =py +Py tai —2a,p,C, -2a,p,S, (PL2-31) 
Phương trình (PL2-3L) được viết ở dạng: 
AC, +BS,=D (PL2-32) 
trong đó: À =2p,a, 
B=2p,a, 
D=p} +p} + a) =a) 
Từ (PL2-32) giải ra được S, và Cụ. 


DB+Ax⁄A?+B?~D? 


Bị Ta? (PL2-33a) 
A?+Bˆ 
-+ ? 2_ m2 
=ẽ....ˆ. (PL2-33b) 
A '.+B 


Từ (PL2-33), với điều kiện: A? + B”~ D?>0, xác định được góc 9: 


Ø, = atan2(%,C) (PL2-34) 
Với góc 8, đã xác định được, theo (PL2-30) ta có các biểu thức: 
—aj 
Su, = L2 2806 S00 (PL2-35a) 
8; 
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Cụ = ¬ (PL2-35b) 
Và từ (PL2-35), xác định được: 

Ô, + Ô; = atan2(S,;,C.;) (€PL2-36) 
Từ (PL2-29e), xác định được d;: 

d; = dị - d, - p, (PL2-37?) 
Từ (PL2-19a,b) xác định được 9; + 9, + 0; 

9,+0; +0, = atan2(S1;„,C¡;„) (PL2-38) 


Sử dụng (PL2-34), (PL2-36) và (PL2-38), sẽ nhận được các biểu thức tính 
các góc Ø,, Ð; và Ø,, 


75 


CHƯƠNG 3 
ĐỘNG HỌC VỊ TRÍ VI SAI ROBOT 


Dịch chuyển vi sai là những chuyển động nhỏ của các bộ phận của robot. 
Dịch chuyển vi sai được sử dụng để xây dựng mới quan hệ tốc độ giữa các bộ 
phận chuyển động khác nhau của robot. 

Trong chương này sẽ trình bày các dịch chuyển vi sai của các khung toạ độ 
tương đối so với khung toạ độ gốc, quan hệ dịch chuyển vi sai giữa các bộ phận 
của robot, ma trận Jacobien và quan hệ tốc độ của tay và các khớp robot. 


3.1. Dịch chuyển vi sai của một khung toạ độ 


Xét một khung toạ độ dịch chuyển nhỏ so với khung toạ độ chuẩn với hai 
trường hợp: không xét và có xét đến cơ cấu gây ra chuyển động. Khi không xét 
đến cơ cấu gây chuyển động, mục này sẽ trình bày dịch chuyển nhỏ của khung 
toạ độ tay và sự thay đổi của ma trận biểu diễn khung toạ độ tay (hình 3.1a). 
Trong trường hợp thứ hai, sẽ nghiên cứu dịch chuyển vi sai của cơ cấu đã gây ra 
chuyển động và tìm mối quan hệ với dịch chuyển của khung toạ độ tay. Hình 
3.1b minh họa chuyển động vi sai của khung toạ độ tay robot gây bởi các 
chuyển động vi sai của mỗi khớp robot. Khi nghiên cứu dịch chuyển nhỏ cần 





Hình 3.1. Dịch chuyển nhỏ của khung toạ độ tay robot: 
a- Dịch chuyển nhỏ của khung toạ độ tay không xét đến cơ cấu chuyển động khớp; 
b- Dịch chuyển của khung toạ độ tay liên hệ với dịch chuyển của khớp robot. 
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phải gắn chuyển động của robot với chuyển động của khung toạ độ tay robot, vì 
các khớp robot dịch chuyển những lượng nhỏ sẽ gây ra địch chuyển nhỏ của tay 
robot cùng với khung toạ độ tay, ví dụ, một robot hàn hai vật với nhau chuyển 
động với tốc độ không đổi. 

Chuyển động vi sai của một khung toạ độ có thể gồm các địch chuyển tịnh 
tiến vị sai, quay vi sai và phép biến đổi vi sai (gồm tịnh tiến và quay). 


3.1.1. Phép tịnh tiến vi sai 


Phép tịnh tiến vị sai là tịnh tiến một khung toa độ các lượng nhỏ, được biểu 
diễn bằng phép biến đổi: 


1 0 0 dx|: 
0 1 0 dy 
T dx, đy, dz) = 
rans (dx, dy, dz) Š*0£. œ 
0001 
3.1.2. Phép quay vi sai 


Xét các phép quay xung quanh các trục x, y, z tương ứng một góc rất nhỏ 
Šx, õy, ôz. Do õx, õy, õz là các lượng rất nhỏ nên có thể coi sinỗx ~ ồx, cosỗx = 
1. Phép biến đổi quay xung quanh các trục x, y, z với các góc rất nhỏ sẽ viết ở 
dạng sau: 


l0 0.0 1 0 8y 0 
0 1 -ðx 0 lo 1900 
Rot(x,ðx) = ,Rot(y,ðy) = 
ĐO GRP Ngrug 1: 0| 02/197 |<Ay 2674: 
0 0 0 1 0 0 01 
Ï —8 00 
ỗz 0 0 
Rot(z,Šz) = : 3-1 
ot(z,Šz) AI: @-Ð) 
0 0 01 


Nếu bỏ qua các tích vô cùng bé: (ỗ”x, õx.õy, ...), dễ dàng nhận thấy rằng 
tích của ba phép biến đổi trên theo thứ tự bất kỳ là như nhau. Điều đó có nghĩa 
là nếu thực hiện ba phép biến đổi (3-1) theo thứ tự bất kỳ sẽ nhận được cùng kết 
quả và bằng: 


7? 


lÐ -ồðz ðy 0 


ổz lI ˆ -ðx 0 
Rot(x,ðx).Rot(y,öy).Rot(z,Šz) = ~ây -:8% L0 (3-2) 
0 0 0 1 


3.1.3. Phép quay ví sai xung quanh vectơ K tổng quát 
Với kết quả (3-2), có thể coi phép quay vi sai xung quanh vectơ k tổng 
quát sẽ gồm ba phép quay đơn theo các trục của hệ toạ độ theo thứ tự bất kỳ với 
các góc quay vi sai ðx, õy, öz. Do đó phép quay xung quanh vectơ K một góc vi 
sai được biểu điễn ở dạng: 
Ì -ðz ðy 0 
ðz ` -ðx 0 
=ðy ðx L0 
0 0 0 1 


Rot(k,đ6) = Rot(x,ðx).Rot(y,Šy).Rot(z,Šz) = (3-3) 


3.1.4. Phép biến đổi vi sai của một khung toa độ 
Phép biến đổi vi sai của một khung toạ độ là sự kết hợp một phép tịnh tiến 
vì sai và một phép quay vi sai tổng quát: 
Trans (dx, dy, đz). Rot (K, d6) 
Xét một khung toạ độ T và dT là dịch chuyển vi sai của khung toạ độ T. 
Khi đó khung toa độ T sau phép dịch chuyển vi sai so với khung toạ độ gốc sẽ ở 
vị trí mới và mô tả bằng ma trận: 
T+đT =[Trans (dx, dy, đz). Rot (k, d6)]T 
hoặc: đT = [Trans (đx, dy, dz). Rot (K, d9) -TỊT 4-4) 
trong đó: I là ma trận đơn vị (4x4), đT biểu thị sự biến đổi của khung toa độ T 
sau phép biến đổi vi sai thực hiện so với khung toạ độ gốc. 
Phương trình (3-4) được viết ở đạng rút gọn: 
đT=A.T (3-5) 
với A = [Trans (dx, dy, đz). Rot ( k, độ) -l] - toán tử vi sai. 
Sử dụng các ma trận biểu diễn phép tịnh tiến vi sai và phép quay vi sai 
(3-3), toán tử vị sai được xác định như sau (3-6): 


100 dx II -ỗz ðy 0 1000 0 -ôz ðöy dx 
l 0 10 dyj|ôz 1! -ðx 0 10100 -| ồZ 0 -ỗx dy 
0 01 dz||-õy ồx 1 0 001 0| |-y õx 0 dd 
000 1 0 0 0 1 0001 0 0 0 0 
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Cần lưu ý rằng, toán tử vị sai không phải là một ma trận phép biến đổi hay 
một khung toạ độ mà chỉ có ý nghĩa đơn thuần là một toán tử và gây ra sự thay 
đổi của một khung toạ độ. 

Ví dụ 3.Ï: Xác định toán tử vị sai tương ứng với phép biến đổi vi sai sau: 

dx = 0,5; dy = 0,3; dz = 0,1 đơn vị và õx = 0,02; õy = 0,04; õz = 0,06 rad. 

Sử dụng (3-6) có: 


0 -006 0/04 0,5 
0,06 0 ~-0/02 043 
-0/04 0,02 0 01 
0 0 0 0 


A= 


Ví dụ 3.2: Xác định địch chuyển của khung toạ độ B và khung toạ độ B ở vị 
trí mới sau phép địch chuyển vi sai gồm một phép quay xung quanh trục y một 
góc 0,1 rad và phép tịnh tiến vi sai Trans((0,1),0,(0,2)). Biết khung toạ độ B là: 


0 0 110 
100 5 
BP lộ: 1203 
000 1 


Toán tử vị sai được xác định từ các dịch chuyển vi sai: 


0 001 01 
0 00 0 
` -01 0 0 02 
0 00 0 


Dịch chuyển của khung toạ độ B được xác định bằng sử dụng (3-4): 


0 001 01]70 0 110] [001 0 04 
-An.|l0 00 0l1900 5| |0 0 09 09 
Dán 0 Dị -01 0 0 02|J010 3| |0 0 -01 -048 @-?) 
0 00 0|000 1| |00 0 9 
Khung toạ độ B mới sau dịch chuyển vi sai là: 
00110] [001 0 04 0 01 1 104 
: _|100 5 00 0 0| |J10 0 5s 
B„.= Ba+ đB=Í0 1 ọ 3|TÍo 6 -0I -0#|“Ío q -01 22 
000 1 0 0 0 0 00 0 1 
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3.1.5. Ý nghĩa của dịch chuyển vi sai của khung toạ độ 
Ma trận đT (3-5) biểu diễn sự thay đổi của khung toạ độ gây ra bởi chuyển 
động vi sai có thể viết ở đạng tổng quát: 
dn, đo, đa đp, 
dn, do, đa, dpy 
dn, do, đa, dp, 
9 0 0 0 


đT= (3-8) 


Mỗi thành phân của ma trận (3-8) sẽ biểu diễn sự thay đổi của các thành 
phần của khung toạ độ. Ví dụ, ma trận (3-7) cho thấy rằng, khung toạ độ B dịch 
chuyển một lượng 0,4 theo trục x; không dịch chuyển theo trục y, dịch chuyển 
một lượng - 0,8 theo trục z. Khung toạ độ không thay đổi theo vectơ ñ; thành 
phần o, thay đổi 0,1 rad; thành phần a, thay đổi — 0,1 rad. 


3.2. Quan hệ dịch chuyển vi sai giữa các khung toa độ 


Toán tử vi sai A (3-6) biểu điễn phép biến đổi ví sai so với khung toạ độ cố 
định chuẩn. Trong trường hợp các phép dịch chuyển vi sai thực hiện so với 
khung toạ độ hiện tại, toán tử vi sai tương ứng sẽ ký hiệu là TA. Do toán tử vị sai 
TA biểu diễn phép biến đổi vi sai so với khung toạ độ hiện tại, sự thay đổi của 
khung toạ độ được xác định bằng nhân phía trước toán tử vi sai ma trận biểu 
diễn khung toạ độ đó. Khi xét dịch chuyển của khung toạ độ trong hai trường 
hợp là như nhau: 

đT=A.T=TA 
từ đó suy ra: TA =(T~),A Œ) (3-9) 

Có thể biểu diễn (3-9) bằng đồ hình trên hình 3.2, trong đó T là phép biến 
. đổi biểu diễn khung toạ độ T. Từ hình 3.2, để xác định toán tử vi sai TA khi biết 
toán tử vị sai A, cần phải xác định phép biến đổi trong dịch chuyển vi sai (T) 
theo nguyên tắc: đi từ mút vectơ đã biết A đến mút vectơ chưa biết TA, 


T 


TẠ 


Hình 3.2. Đồ hình mô tả phép dịch chuyển ví sai. 
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Ví dụ 3.3: Cho đồ hình như hình 3.3, xác định phép biến đổi trong dịch 
chuyển vi sai và toán tử vị sai °A khi biết A. 


A 


A ^A BA Hinh 3.3. Đồ hình phép dịch 
chuyển vì sai. 


Phép biến đổi trong dịch chuyển vi sai là T = (A.B). 

Từ đó toán tử vi sai 8A đựơc xác định như sau: 

BA =TA.T = (A.B)!.A (A.B) = (B†!.A).A (A.B) 

Phương trình (3-9) biểu diễn quan hệ toán tử vi sai thực hiện so với hai 
khung toạ độ, và được sử dụng để xác định toán tử vi sai so với khung toạ độ 
hiện tại (TA) khi biết toán tử dịch chuyển vi sai so với khung toạ độ chuẩn cố 
định (A). Sử dụng dạng tổng quát của ma trận T, T"' và toán tử vi sai A, từ (3-9), 
toán tử vi sai TA được xác định như sau: 


x-.-. Bác 0 -ðz õy dxi|n, 0, 4, Ðy 
TẠAI Sa y 5s — ðö 0 -ôx dy|l|n, 0y 4v Py 
a, Ay 4, -BÃ -_ðy ồx 0 dzlln o a, P, 

0 0 1 0 0 0 0]0 0 0 1 


(3-10) 
Sau một số phép biến đổi, toán tử vi sai so với khung toạ độ T có dạng: 
0 - 8z Tậy Tdx 


T = T8; 0 -Fãx Tdy 


-ây Tậy 0 Tqz GẦN 
0 0 0 0 
với: Tã,= ồm; 
T8,= ỗØ; 
T§ = Šấ; (3-12) 


8l 


Ví dụ 3.4: Xác định toán tử vi sai °A cho ví dụ 3.2, 

Từ ma trận B đã cho, viết được các vectơ ïï, ð, 8, B 

Và các vectơ dịch chuyển vi sai quay và tịnh tiến ỗ,đd như sau: 
ñ =[010]?; ø =[00 I]Ÿ; ã =[100]7; p = [10 5 3]; 
Š =[00,1 0]; đ ={0,100,2]P 


j k 
Šxp=|l0 01 0|=[030-I0] 
l0 5 3 


Šx + d=[0,3 0-1,0]Ï + [0,1 00,2]T = [0,4 0 -0,8]7 
54 =ñ.{Šx +d]=0.0,4+1.0+0.(- 0,8) =0 


5d, = 


5| 


øI 


.[ðx +d]=0.0,4+0.0+ 1. (-0,8)=- 0,8 
%3= ã.[Šx +d]=1.04+0.0+0.(—0,8)=0,4 
58 = ôñ =0.0+0,1.1+0.0=0/1 

Đã ,= ðø =0,0+0,1.0+0.1=0 

°,= ỗã =0.1+0,1.0+0.0=0 

Như vậy:?a =[ 0, (- 0,8), (0,4]F 

°Š =[(0,1),0, 0 J 


0 0 0 0] 
0 0 ~01 -08 
B ” » 
` Â*Ìo ¿0i 0 04 
0 0 0 9 


3.3. Quan hệ dịch chuyển vi sai của robot trong 
không gian làm việc 


Xét một robot sáu bậc tự do trong không gian làm việc như hình 3.4. Sử 
dụng khái niệm đồ hình như hình 3.2, có thể xây dựng đồ hình mô tả dịch 
chuyển vi sai trong robot như hình 3.5, Trên hình 3.4, z là phép biến đổi biểu 
điển robot trong hệ toạ độ gốc, Tạ biểu diễn tay robot so với khung toạ độ robot, 
E biểu điễn điểm kẹp tay robot so với khung toạ độ tay, B mô tả chỉ tiết trong 


82 


khung toạ độ chuấn, và Œ mô tả điêm kẹp trên chị tiết. Các toán tứ ví sai 
AA.A, °A( SA), 5A được thực hiện tương ứng so với khung toạ độ gốc, robot, 
tay, điểm kẹp, và chỉ tiết. 





Hinh 3.5. Đồ hình phép dịch chuyển vi sai trong robot. 


Quan hệ các phép biến đổi thể hiện bằng phương trình sau: 
Z.T¿.E= B.G (3-13) 
Từ đồ hình 3.5, có thể xác định dịch chuyển vi sai so với khung toa độ, gây 
ra bởi địch chuyển vi sai của khung toạ độ khác. Ví dụ, có thể dịch chuyển vi 
sai so với khung toạ độ gốc ( ”A) gây ra bởi dịch chuyến vi sai so với khung toạ 


độ T„( 5A ) bằng sử dụng phép biến đổi trong dịch chuyển vi sai: 


T=T¿1Z' (3-14a) 
hoặc: T=EG'B' (-14b) 
Toán tử vi sai so với khung toạ độ gốc được tính tương ứng như sau: 
A=T!,*A.T 
=(T/!Z91!.5A .(Tg1/29) 
=Z. Tạ. *A.T.!/Z1 (3-15) 
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Ví dụ 3.5: Một camera gắn trên thanh nối 5 của robot 6 bậc tự do được mô 
tả bằng ma trận sau: 


900 -IlI § 
0 -Ì 0 0 
h5 CAM= 
-Il 0 0 10 
0 0 0 1 
Ma trận mô tả thanh nối cuối cùng so với thanh nối 5 của robot có dạng: 
0 -I 0 0 
L0 00 
A,= 
0 0 1 68 
0 0 0 j¡ 


Một đối tượng “^⁄Q được quan sát bằng camera và dịch chuyển trong 
khung toa độ CAM bằng các vectơ dịch chuyển vi sai. 

CAMđ =-1,1 +1,J +0.k và “^“§ =0.1 +0.j +0,1.K 
Xác định dịch chuyển vi sai trong khung toạ độ Tụ, 





T; À, E X O CAM T, 


Hinh 3.6. Đổ hình phép dịch chuyển vi sai trong robot. 


Hình 3.6 trình bày quan hệ các phép biến đổi trong robot. Quan hệ đó thể 
hiện bằng phương trình: 
T;.A,.E.X = T;CAM.O 
trong đó: T; - phép biển đổi biểu diễn thanh 5 so với khung toạ độ thân robot; 
A¿ - phép biển đổi diễn thanh 6 so với khung toạ độ thanh 5; 
E - phép biển đổi biểu diễn bàn tay robot; 
X- phép biến đổi chưa biết mô tả đối tượng so với bàn tay robot; 
© - phép biến đổi mô tả đối tượng trong khung toạ độ camera. 
Từ đồ hình 3.6 xác định được phép biến đổi trong địch chuyển vi sai là: 
T= CAM', Tz!, Tạ.Á, = CAMA, 
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0 0 -—I 10110 1l 0 0 0 0 ~1 2 
T. 0 -I 0 01|)1 000 s -l 0 0 0 
-l 0 0 5SI|0 0 1 8 0 1 0 
0 0 0 11410 0 01 0 0 0 1 


Sử dụng ký hiệu tổng quát của ma trận T, ta có: 
ñ ={0-100]Ÿ; ø =[0010]7; 
ã =[-1000ÏÏ; ø =[20 5 1Ï) 

Áp dụng (3-12) có: 


ml 


=0.i+0,2.]+0.k 


©i 
"“ 
ơi 
lì 
k2 G mí 
®&œc«.e-i 
= 


ŠxP+đ=-1.ï+12.J+0.k 


và TSđ =-1,2.1 +0.) +l.k 
+ 


3.4. Ma trận Jacobien 
3.4.1. Khái niệm ma trận Jacobien 


Giả sử tổn tại một vectơ ïị = [n,, ...rị, ÏÏ và một vectơ š =[§,,...É, ]T, 


trong đó: nị, = f(Š,,...Š,) và rị, Š; là hàm của thời gian và có đạo hàm cấp l. 
Khi đó: 





dt ốc dt ốc dị 7” di 
: -16) 
đn, ỡn, đề, ở, đề, ở, đề, 
——=——+———~+....+———~ 
đẻ ô, dị 6, dị Øš, dị 
hoặc viết ở dạng ma trận: 
êN ên êm 
- |lâ j5 
KIẾP " n k BỘ (-17) 
dt ôn, ôn, ôn, dt 
&. Đụ , 


Ký hiệu ma trận (1 x K) là !„(E ) - ma trận Jacobien của mị đối với Š : 
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Jạ(šŠ)= : (3-18) 
ôn ổn ôm, 
KH” , 
Sử dụng (3-18), (3-17) được viết lại ở dạng: 
T6 @-19) 


(3-19) cho n rằng ma Tim Jacobien biểu thị mối quan hệ về đạo hàm cấp 
một của hai biến Tị, š. Nếu xét trong khoảng thời gian hữu hạn, phương trình 
(3-16) sẽ được biểu diễn ở dưới dạng phương trình sai phân, hoặc là phương 
trình của các thay đổi nhỏ: 


ồn, lấn mm, .BỆ,+..... + ạy 
%, y ô 
(3-20) 
ôn, “ghê _ #iuy- _ 
Tương tự, phương trình (3-19) được viết ở đạng phương trình sai phân: 
Šf =J„(€).êš -21) 


3.4.2. Ma trận Jacobien trong robot 


Sử dụng khái niệm ma trận Jacobien cho cơ cấu robot, có thể viết phương 
trình biểu diễn quan hệ chuyển dịch vi sai của tay robot và các khớp robot 6 bậc 
tự đo. 


dx dq, 
dy Ma trận n 
2Í < | đaagban || SG (-22a) 
õx dq, 
robot 
ðy dq, 
ðz dq, 
hoặc: D=J.D, (3-22b) 
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trong đó: dx, dy, đz là các dịch chuyển vi sai của tay robot tương ứng dọc theo 
các trục X, Y, Z; 

ðx, ôy, ỗz tương ứng là các góc quay vi sai của tay xung quanh các trục 
X,Y, 2; 

D: vectơ dịch chuyển vi sai của tay Robot và Dạ là vectơ dịch chuyển 
vi sai của khớp. 

Nếu chia cả 2 vế (3-22a) cho dt, sẽ nhận được phương trình quan hệ tốc độ 

của tay robot và tốc độ khớp. 


3.4.3. Xác định Ma trận Jacobien 


Phương trình động học thuận biểu thị quan hệ giữa vị trí và hướng của tay 
robot và. vị trí của khớp. Do đó theo (3-22a), mỗi thành phần của ma trận 
_ Jacobien sẽ là đạo hàm phương trình động học tương ứng theo mỗi biến. Từ 
(3-22a) thấy rằng phần tử thứ nhất của D !à dx, do đó phương trình động học 
thứ nhất sẽ biểu thị dịch chuyển đọc theo trục x - tức là p, trong ma trận T tổng 
quát. Tương tự như vậy đối với các thành phần dy, dz. Từ ma trận T biểu diễn 
tay robot, lấy đạo hàm p,„p, và p, sẽ tương ứng được dx, dy, đz. 
Xét một robot có cấu hình như hình 2.21, cột thứ tư ma trận của phương 
trình động học thuận của Robot có đạng (xem ví dụ 2.15). 


P. CC 4, +Ca, +C;a,) 

P, |_| 3,(Cz¿4, +Caya; +C;a,) (3-23) 
P, S;yâ. Ma +8yA, 

1 " 


Tính đạo hàm p,, nhận được phương trình sai phân sau: 
p ốp ôp 
dp, =—*dô9, +——*d9, +...+——*>d8 
Ô0, I : 2 %, 6 








dp, = -5,(Ca„a, + Cua; + Ca,)dÕ, + CCCS;x¿á¿ - S;;a; - S;a,)dÖ; + 
€;S;x¿a, - S;;a:)dÖ; + C¡(-S;;„a„) dÔ, 
Từ đó nhận được các thành phần của hàng I ma trận Jacobien: 
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1ị,= =-S,(C,,ua, +C,¡8; +C;a;) 





1= ==C¡CSzy¿4¡ 8,4; =8;4;) 


a 
ôp . 
1y = =CiCŠ;a, —Š„;4; ) 
3 
ốp 


23 


(3-24) 





Jụ ". =C(C§„,a„) 
4 
?, 
đụng 
5 
ốp 
Jụ S ng 


Tương tự, có thể tính được các thành phần 2 hàng tiếp theo của ma trận 
Jacobien: 


ðp 
1, =—=~=C/(C;„a, +C;;a; +C;a;) 
3, 
%, 
1; "ThS, —8„4; —8;A;) 
ôp 
J “=5 8x4, —8„;a; ) (3-25) 
- 
ðp 
Jụ “=8 844) 
4 
ÔP, 
J,„ =>: YEy 
$ 
ở, 
1y ` cử 


e 
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1y =0n=0 
I 
ÔP, 
J;; =ng T Ca, +C,;a; +C;a, 
? 
GP, 
J;, ao =C,a„+C,;4, 
3 (3-26) 
©p, : 
Jụ "a =C,uA, 
4 
J -% =0 
36.” a x 
; 
Ôp, 
Tự suy 


Phương trình (3-23) biểu thị vị trí của tay Robot với các biến khớp (q), do 
đó, sẽ xác định được các dịch chuyển tịnh tiến theo các trục thông qua các biểu 
thức tính các thành phần của ma trận Jacobien liên quan đến dịch chuyển tịnh 
tiến. Mặt khác không tồn tại phương trình duy nhất mô tả sự quay xung quanh 
các trục, đo đó sẽ không có phương trình duy nhất cho tính toán õx, ðy, ỗz. 

Ma trận Jacobien so với khung toạ độ gốc sẽ được tính đơn giản hơn thông 
qua tính ma trận Jacobien so với khung toa độ tay (T,) sau đó tính quy đổi về 
khung toa độ gốc [TL Pani ]. Ma trận Jacobien so với khung toa độ Tự biểu thị 
quan hệ địch chuyển vi sai của tay tương đối so với khung toạ độ Tạ và các dịch 
chuyển nhỏ của các khớp: 

SP J5D, @-27) 


Phương trình quan hệ dịch chuyển vì sai giữa khung toạ độ tay Tự và các 
khớp được viết ở dạng chỉ tiết: 


T§ T Tụ 
T dx : J1ị n Jịụ; .... ` J1 dq, 
T. đy _ Jặn h 1» ... x J„ dq, 
Tụ đz = ST s Sa ghê sư dq; (3-28) 
® 8x 1 : kÔ: dq 
T 4t l vn 46 4 
VÖY| |*Jng Ì «~ *Jy dq, 
"ÑJ. |ẤJ, ÿ ˆ.„ SJ,|JUN 


trong đó: '*J,.=%dx,;*J„;= *ầx, - tương ứng là độ dịch chuyển vì sai và góc 
quay vi sai của tay theo trục n gây ra bởi dịch chuyển của khớp ï; 
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“* 1= *dy,;* J„¡=`*êy, - tương ứng là độ dịch chuyển vì sai và góc 
quay vi sai của tay theo trục o gây ra bởi dịch chuyển của khớp i; 

- dz;*J„=% ðz¡ - tương ứng là độ dịch chuyển vi sai và góc 
quay vi sai của tay theo trục a gây ra bởi dịch chuyển cửa khớp ¡. 


Các thành phân '* dx,; 5 dy,;'s dz,;*ôx,;'sõy ¡+* Šz, được tính theo công 
thức (3-12) được xét cụ thể trong hai trường hợp khớp quay và khớp tịnh tiến: 

+ Đối với khớp ¡ là khớp quay: d, = 0 và 9, của khung toạ độ ¡ là góc quay 
xung quanh trục z,¡ và ổ, =[0 0 I]T. Do đó áp dụng (3-12) có: 


XiJy=5 dX; =—n,¡Py¡ +R¡Ð,¡ 
ụ=" dy;, =~0,¡P„¡ +O¡P„¡ 
h Jạị= *đz, =-â,iÐyi †8v¡P„i 
⁄ lạ= 5ô, =1 
5Jy=Xây,=o„ 
5 Jg=ôz, =â„ 


(3-29) 


+ Đối với khớp tịnh tiến: ô, =0, độ dịch chuyển tịnh tiến là dịch chuyển 
dọc theo trục z,¡ và đ, =[0 0 I]”. Do đó: 
` 1ụ= *đx, =n, 
h Jại = dy; =0, 
_ Jạ = đZ, =â„ 


3-30 
%J„=Š%§x; =0 Kiện: 
*J„=%ây, =0 
hs Jạ¡=”säz, =0 


Trong (3-29) và (3-30), các vectơ ï, =Ín„ tự n„Ì;ð, =b„ Đụ 9Ï 


t 


ñ, =Ă„ âu „| VÀ Đ, ¬ Đụ p„Ì'à các cột của ma trận phép biến 
đổi T, trong dịch chuyển vi sai được xác định dựa trên nguyên lý phép biến đổi: 
dịch chuyển của khung toạ độ thanh thứ ¡ được thực hiện so với khung toạ độ 
thanh ¡-1 như minh hoạ trên hình 3.7: T, = ''T; = A,.Á.„;...À, -31) 
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Hình 3.7. Phép biến đổi dịch chuyển ví sai. 


Ví dụ 3.5: Xác định các thành phần "J,, và T5J,. của ma trận Jacobien cho 
robot ở hình 2.21. 

Ma trận phép biến đổi trong dịch chuyển vi sai dùng cho tính T#J,, và 15J 
xác định theo (3-31) và ví dụ 2.15. 


Bựi Đựi đụ Pài 
: HN. a 
T,=°T, = A,A;A;A,A;A, =| nh SH ân đạ 
Hạ Đại đạy Đại 
00 0 1 
C¡(CjC¿C¿ 5254) CICCC¿C;C,TS2,C¿) CjCa¿§, C,(Cz;j.a. + 
—5¡5,Œ, +8¡S;5„ +8,C€; +¿;a; +C¿.ã;) 
S¡@z;C,C; —8„zz5,) S¡Cz,C,C, —8„„C;) S¡Czx+Ÿ; S¡(C; a;? 
+C¡§,5, -€,5;5, ~C,C€; +C¿;a; +C;.a¿) 
S;¿C;C + Ca¿5¿ —§z„„C;C¿ +C„„C¿ S5 5; (A¿ + 


+S;y4; +Š;.a; 


9 0 0 1 
Sử dụng (3-29) cho khớp 1 là khớp quay, nhận được: 
Ty= x1.IĐyi + Hyi Dạ; 
= -[C((Ca¿C¿Cs- S252) - S¡5sC¿].[S,(C;a¿ä, + Cạa‡ + C;a;)] 
+ [5,(Ca¿C¿C- 52„5,) - C¡S2Cs]-[C¡(Cz„a+ Cạaa; + Cạa )] 
= 8;C2(C2;¿a¿ +Cạya; + Cạa;) 


TS; = nạ = S;¿¿C¿C¿ + CayŠ 
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3.5. Quan hệ ma trận Jacobien và toán tử vi sai 


Giả sử các khớp robot dịch chuyển các lượng nhỏ, sử dụng (3-22a) với ma 
trận Jacobien đã biết, sẽ tính được vectơ địch chuyển vi sai D của tay robot gồm 
các thành phần dx, dy, đz, ôx, öy, õz. Thay các thành phần địch chuyển vi sai 
của tay robot vào (3-6) sẽ nhận được toán tử vi sai. Sử dụng (3-5) sẽ tính được 
dịch chuyển của khung toạ độ tay đT của robot. Độ địch chuyển đT được sử 
dụng để đặt tay ở vị trí mới sau địch chuyển vi sai. 

Theo phương pháp khác, theo (3-27) với ma trận Jacobien so với khung toa 
độ Tạ, xác định được vectơ dịch chuyển của tay robot so với khung toạ độ tay 
robot (”*) gồm các thành phần "4x, Tqy, T4; T6ạy, Ty T68 Thay vào 
(3-11) sẽ nhận được toán tử vi sai so với khung toạ độ Tạ: "5A. Cuối cùng sử 
dụng (3-9) sẽ xác định dịch chuyển đT của khung toa độ tay robot. 


Ví dụ 3.6: Tính vị trí mới của tay robot 5 bậc tự do sau các phép dịch 
chuyển vị sai của các khớp robot: 


d6] [ 01 
d62l |~01 
D; =| dđ3 | =| 0,05 
đ04| | 04 
d95J | 0 


Biết ma trận biểu diễn tay robot trong khung toạ độ gốc và ma trận 
Jacobien như sau: 


3 0000 

0 0 5 
Tin hổ -2 0100 
1# ly ng ạ 2| 25100-4:9 099 
0 1 010 

0 0 0 1 
~1 0001 


Giả sử robot chỉ quay theo các trục x, y, vectơ dịch chuyển vi sai của tay 
._ robot được xác định theo (3-22a): 
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dx 3 
dy -2 
B=| dz |= Dạ =| 0 
õx 0 
ỗy ~1l 00 
Do đó toán tử vi sai được viết như sau: 
0 0 -0I1 03 
A£ 0 0 0 -015 
01 0 0 -0,4 
0 0 0 0 
Sử dụng (3-5) ta có: 
9 


— >> CC CC 

= c —cœ 

© —= c CC 
© 


-01 03 
0 -0/15 
0 -04 


0 0 


đT, = AT, =| 0 


0 


©  cc 
© c c — 


0,1 

olÌ—o,I 

0,05 
01 


—~ t3) Q2 {nh 


03 
—0/15 
=| 0,4 


01 


Vị trí mới của khung toa độ tay robot sau khi dịch chuyển vi sai là: 


0 -01 0 0j 
0 0 0 -015 
01 0 0 0 
0 0 0 09 

lL -01 0 5SI 

_]|0 0 -I 285 

“|0 1 0 241 

0 0 0 1 


Tạ =đT, + Tạ ~ 


3.6. Ma trận Jacobien nghịch đảo 


1 0 0 § 
0 0 -l 3 
0 I 0 2 
0 0 0 1 


Để xác định dịch chuyển vi sai, hoặc tốc độ cần thiết của các khớp robot 
tương ứng với địch chuyển vi sai hoặc tốc độ của tay robot, cần phải tính ma 


trận Jacobien nghịch đảo theo phương trình sau: 
D=1D, 


Từ đó nhận được: D, =J".D 


@-32) 
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Tương tự: D.= "1P (3-33) 


Phương pháp tính theo (3-32) và (3-33) phức tạp và thời gian tính toán lớn, 
nên ít được dùng trong thực tế. Sau đây trình bày phương pháp tính toán các 
địch chuyển vi sai hoặc tốc độ của robot có cấu hình ở hình 2.21 bằng sử dụng 
phương trình động học ngược đã dẫn ra ở mục 2.7. 

Sử dụng phương trình (2-43a): 


S,.p,-C¡.p, =0 (3-34) 
Vi phân phương trình (3-34), sẽ tìm được dÔ,, 
Sị.P, = Cị-Py 
C¡.p,d0; + S.dp, = -S,.p,d9, + Cụ dp, 
Suyra: dÔ,= bbx + NHI (3-35) 
Cịp, + Sidp, 


Sử dụng phương trình (2-52): 


S2,„C¡a, + 5¡ay) = C;yua, = 0 (3-36) 
Vì phân (3-36) ta có: 
©,z(d6; + d9; + đ6,) (Cịa, + Siay) + S2„(-5,a,d0, + C¡avd6, + C¡da, + S,da,) 
=-S,;„(d6; + d0; + dÓ,)a, + C;;„da, (3-37) 
Do đó ta có: 
d9, + đÔ, + dÔ, = S(-Sia,đ6, + Ca d6, + Cua, + Sida,) + C,da, 
Suyy3, xp C,,(C¡a, + Say) 
(3-38) 
Trong phương trình (3-38), các vi phân da,, da, và da, đã được xác định từ 
phương trình biểu diễn vi phân đT: 


¿ 3-39) 


Tiếp theo vi phân phương trình (2-48) để tìm dÔ:, 
2a;a;C; .= (Cp, + SỊPy —C¡yâ, là + (P, “g S„„a,)Ÿ -8) = 83 (3-40) 
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—2a;a;S;d9, = 2(C¡p, +ŠJÐy —C¿;zâ„) 
x[C;dp, —S¡p,dÓ, + S,dp, +Cip, +a,5;;„(đ9; + đÔ; + d6, )] 
+2, —5;;;4,) [dp, —a,C;;;(đ9; + đÔ; + dÔ,)] 


(3-41) 
Tương tự, vi phân phương trình (2-56) sẽ xác định được đ6,. 


S;[(Cạa; +a;)Ÿ +S3a‡]= (Ca; +az)(Ð„ —Sas¿a¿)—S;a;(p„C¡ +PyŠ —k x34) 
@-42) 
C;đ62[(Csa +a;)” + Sia3]+ 2S;[(Ca; + aa)(—asS;dÔs ) + 2a2S;C;d6; ] 
=~asSadÐ3(P„ —4aSas4) + (42C; + a2)[dp; ~aaCzsz(đ9; + d6 + dôx )] 
—83€¿;dÐ;(p,C¡ + pyŠ¡ — C2344) 
—as53[C¡dp, — p„5¡d8) + Sidpy +pyC¡đÕi +aaS2‡4(đ6; + đô; + đÔx)] 


(3-43) 
Sử dụng (3-38), (3-41) và (3-43) sẽ xác định được đ0,, d6; và d0,. 
Vị phân phương trình (2-59b) sẽ nhận được công thức tính đ0.. 
C; = -Cịa, + Sia, 3-44) 
-8;d0; = a,S,d0, —C;đa, + a,C.dÔ, +S,da, (3-45) 


Cuối cùng sử dụng phương trình cân bằng hai phần từ (2,1) hai vế để tính ra 
dÔ,: 
5= - 5;„(Cun, + S¡n,) + Can, (3-46) 
Vị phân phương trình (3-46) nhận được phương trình để giải được đ6,. 
CzdÕ, = -C;„(C¡n, + Sạn, )(d6; +d6; + d6,) 
~ 8;z8,n,dØ, +C¡dn, + C,n,d9, +S,dn,) (3-47) 
- nz32;,(d9; +d0; + đ0,) + C.;udn, 

Như vậy bằng 6 phương trình vi phân sẽ xác định 6 giá trị vì sai vị trí của 
các khớp. 6 phương trình đó sẽ được sử dụng để lập trình điều khiển robot và 
tính tốc độ của các khớp robot. 

Ví dt 3.7: Cho robot có cấu hình như hình 3.8 (xem ví dụ 2.15). Tính tốc 
độ của khớp 1 khi biết tốc độ đài và tốc độ góc của tay robot như sau: 

'dx/dt = 2,4 cm/s ; dy/dt = 4,8 cm/s ; ðx/dt = 0,1 rad/s. 
0,=0'; 0;= 90; Ô;= 0°; 8,= 90"; 0,= 0°; 0,= 45° 
a; = 0,5 m; a¿ = 0,5 m ; a¿= 0,1 m. 
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Hình 3.8. Cấu hình và các khung toạ độ của thanh nối 
robot dạng khớp nối 6 bậc tự do. 


Bảng 3.2. Bảng D — H của robot 





Sử dụng kết quả của ví dụ 2.15, thay giá trị các vị trí các khớp ở bảng 
D— H vào phương trình động học thuận robot (xem ví dụ 2.15), với giả thiết vị 
trí của robot được lấy mốc theo trục x, sẽ tính được ma trận *T;, biểu diễn vị trí 


và hướng cuối cùng của tay robot: 


TT c SÁU AA )AL^A;2A¿ 
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€¡(C2j2C¿C¿ —S23S¿) CỊ(CC¿;zC;C¿ T-§S¿;zC¿) C¡C¿¿¿3; C¡(C¿2jy4¿ + 


—S¡5:C, +ŠIS:Š; +S§,C; +€C¿;3a‡; +C¿2.4;) 
SI(C2j2CsC¿ —523254)  SI(CCC¿C¿ ~S5z¿C¿) SC¿j5; S¡(C¿s‡.a¿ + 
ĐỆ ¡8% -CS4S “CƠ, +Cayay+Cj22) 
522CsC¿ + C23454) —Sa„C;C¿ +C¿zC¿) - S¿x¿§; - S;;y¿a¿ + 
+S¿‡ãi +Š+.4; 
0 0 0 1 
n, 0y 4, P„¿] [-0707 0707 0 -01 
_|Hy 0y ây pyị- 0 0, -1l 0 
_Ín, o„ a, p„ị |-0/707 -0707 0 1 
0 0 0 I1 0 0 0 1 


Từ các giá trị tốc độ dịch chuyển tịnh tiến và dịch chuyển quay, tính được 
toán tử vi sai theo công thức (3-6): 
0 -ðôz ðy dx 0 00 0,024 
A-|ŠZ 0 -8x dy| |0 0 -01 -0/048 
=ỗy ðỗx 9 dz 0 01 0 9 
0 9 0 0 9 0 0 0 


Từ đó xác định được ma trận dịch chuyển vi sai của T: 


0 0 0 0,024 
đT=AT= 0/0707 0/0707 0 —0,148 

0 0 9 0 

0 0 0 ọ 


Do đó: dp, = 0,024; dp, = -0,148 ; dp, = 0; 
Theo công thức (3-35) tính được: 
dô, _ Cidpy —S¡dp„ _ 1(—0,148)— 0.(0,024) 


dt TCip,+tSpy  1LC0))+0C0248) HH 
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Bài tập 


1.Vị trí và hướng tay robot được mô tả bởi ma trận “T;, dưới đây. Thực hiện 
quay vi sai xung quanh trục x một góc 0,15 rađ; tiếp theo là phép dịch 
chuyển tịnh tiến vi sai [(0,1),(0,1),(0,3)]”. Xác định vị trí mới của tay robot. 


0 0 1 2 
1 00 7 
0 190 5 
0 0 01 
2. Cho ma trận A biểu diễn 1 khung toạ độ. Khung toạ độ A dịch chuyển tịnh 
tiến bởi vectơ đ = [(),(0),(0,5)]T và vectơ quay  = [(0),(0,1),(0) 
+ Xác định toán tử dịch chuyển vi sai so với khung toạ độ gốc. 
+ Xác định toán tử địch chuyển ví sai so với khung toạ độ A. 


RT..= 


0 0 1 10 

I 0 9 s5 
A= 

0 100 

0 001 


3. Cho ma trận biểu điễn khung toạ độ tay robot và ma trận Jacobien tương ứng. 
Vectơ dịch chuyển nhỏ của các khớp là D.. Xác định toán tử địch chuyển vi 
sai và độ dịch chuyển của khung toa độ tay và vị trí mới của khung toạ độ 
tay. 


Biết: 
§ 00000 0 
0 1 0 10 -3 0 1000 0,1 
T,= 1 0 0 § T6 0 100000 B- =0, 
0 0 ~l1 0 0 1 001 0| " 0,2 
0 0 0 1 0 0 0100 0,2 
-l 0 0000 0 


4. Cho ma trận T biểu diễn khung toạ độ T và đT là ma trận dịch chuyển vì sai 
của T. Xác định vectơ dịch chuyển tịnh tiến đ, vectơ dịch chuyển quay ỗ và 
toán tử dịch chuyển vi sai A. 
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Biết: 


1 0 0 s5 0 -01 -01 0,6 
T= 0 0 13 đT= 01 0 0 0,5 

0 -1 0 § -01 0 0 -0,5 

0 0 01 0 0 0 9 


5. Ma trận Tị, T¿ biểu diến khung toạ độ tay robot tương ứng ở vị trí ban đầu và 
vị trí cuối cùng. 


1 0 909 5 I1 0 01 48 
0 0 -1 3 01 0 -—1 3,5 
đc TS 
0 1 0 6ó 0 1 0 62 
0 0 0 1 0 0 9 1 
+ Xác định ma trận biến đổi Q. 


+ Giả sử các biến là nhỏ, xác định toán tử dịch chuyển vi sai. 
+ Xác định vectơ dịch chuyển tịnh tiến và quay vi sai. 
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CHƯƠNG 4 
ĐỘNG LỰC HỌC ROBOT 


4.1. Bài toán động lực học 


Ở các chương trước, đã nghiên cứu động học vị trí và động học vi sai, cấu 
trúc hình học và các chuyển động robot, nhưng chưa xem xét đến các lực gây ra 
các chuyển động. Trong quá trình di chuyển, robot tiếp xúc với môi trường sẽ 
sinh ra một lực cần thiết để đi chuyển vật và thực hiện công việc, ví dụ: robot 
lắp rấp mang một chỉ tiết đến vị trí định trước lắp ráp vào một chỉ tiết máy khác. 

Lực cần thiết tác động lên một vật thể quan hệ với gia tốc của vật thể theo 
quan hệ sau: 


= F=ma 
Tương tự mômen quay của một vật quan hệ với gia tốc góc của vật thể là: 
>M=1a 
Để làm khớp robot di chuyển tịnh tiến hoặc quay, cơ cấu chấp hành cần 
sinh một lực hoặc mômen đủ lớn. Mối quan hệ giữa lực, mômen của các khớp 
với vị trí, tốc độ và gia tốc được biểu diễn trong phương trình chuyển động, còn 
gọi là phương trình động lực học. Trong phương trình động lực học, lực và 
mômen là các tín hiệu vào. Dựa vào phương trình động lực học, sẽ tính được 
lực, mômen cần thiết để khớp robot có thể chuyển động được với tốc độ và gia 
tốc mong muốn. Phương trình động lực học cũng được sử dụng cho đánh giá 
ảnh hưởng của khối lượng tải đối với các chuyển động của robot. Giải phương 
trình động lực học sẽ nhận được các chuyển động của robot với các đầu vào là 
lực và mômen của các khớp, là cơ sở cho thiết kế hệ thống điều khiển robot. 
Trong chương này, sẽ trình bày phương pháp Lagrange và sử dụng phương 
pháp đó để xây dựng phương trình động lực học robot 2 thanh nối. 
Trên cơ sở đó sẽ dẫn ra hệ phương trình động lực học của robot n thanh 
nối. Tiếp theo sẽ xây dựng các mô hình toán học robot. 
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4.2. Phương trình Lagrange 


Trong phương trình Lagrange, các biến toạ độ tổng quát được sử dụng để 
xác định duy nhất vị trí của vật. Phương trình Lagrange sẽ nhận được từ phương 
trình Niutơn. 

Giả sử hệ thống gồm n phần tử vật chất với n bậc tự do mô tả bởi các biến 
toạ độ tổng quát q¡, q, ..., q, (đối với chuyển động khớp quay, toạ độ tổng quát 
là góc quay của khớp q, = 6¿ đối với khớp chuyển động tịnh tiến, toạ độ tổng 
quát là độ di chuyển q, = d,). Một vectơ vị trí trong không gian 3 chiều X,„ đối 
với mỗi phần tử vật chất P„ được ký hiệu là: 

K„ =X„(Q¡;4;.~5q,:£) (4-1) 

khối lượng của mỗi phần tử vật chất P„ là m, và lực đặt lên P, là F,, 
phương trình chuyển động Niutơn có đạng: 

F,=m,Ñ, (4-2) 





% : l 
Lấy đạo hàm ———- và lấy tổng đối với tất cả các phần tử vật chất của hệ 


thống, ta nhận được c trình sau: 








a 
>R ñ m _=šm,x Và :‹q= L,2,..., 1 (4-3) 
Mặt khác, § phương trình (4-1) ta có: 
TỔN, 
=3 —ä,; (4-4) 
íl ai ` 
ụ ụ 
=. (4-5) 
öđ; ôq, 
Như vậy phương trình (4-3) có thể được viết lại ở dạng sau: 
%& |} %K 
=—|—|-— 4-6 
s dt ta l Ôq; : l 


ffc 1v, 
trong đó: K = 3g Xấu - động năng của hệ thống; 
R 


=rỖ, : ỳ „ : 
và Q, =3F„ —— - lực tổng quát tương ứng với toạ độ q,. 
u ôa 
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Lực F, gồm hai thành phân: thành phần trọng lực (F.„) và thành phần các 


da 
lực còn lại (F y ). Thành phần trọng lực được tính từ thế năng của phần tử: 


mm. T 


Hà 
ụ 


Khi đó nhận được thành phần Q;, có dạng sau: 


=r ỐNG 
Qụ =3, lộ Â. (4-8) 
Hàm Lagrange là hiệu của động năng và thế năng của vật có dạng: 
L=K-P (4-9) 


Sử dụng các biểu thức (4-6) + (4-8), sau một số phép biến đổi, phương trình 
chuyển động Lagrange được viết như sau: 
dị ôL ôL 
Q,=12 |” (4-10) 
dtLôđ,/ ôn, 
Đối với vật thể cứng có n bậc tự do, động năng của vật K được xác định 
như sau: 


K=-mvfy+le1Jø (4-11) 
2 2 
trong đó: v - tốc độ chuyển động tịnh tiến của tâm vật thể; 
œ - tốc độ góc; 


m - khối lượng vật thể; 
J, - mômen quán tính của vật thể. 


4.3. Phương trình động lực học của cơ cấu robot 
2 thanh nối 


4.3.1. Xây dựng phương trình động học 


Xét một cơ cấu robot 2 thanh nối trong hệ toạ độ 2 trục Oxy như trên hình 
vẽ 4.I. 

Ký hiệu các tham số trên hình 4.1 như sau: 

Õ, - góc quay của khớp quay ¡ (¡ = 1, 2); 

m, - khối lượng của thanh nối ¡; 
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Jạ - mômen quán tính của 
thanh nối ¡ đối với trục qua 
tâm khối của thanh nối; 

1,- chiều dài của thanh nối ì; 

I„ - chiêu dài từ khớp ¡ đến tâm 
khối thanh nối ¡. 

Giả thiết khớp L sinh ra 
mômen M, tác dụng giữa bệ và 
thanh nối 1; khớp 2 sinh ra mômen 
M; tác dụng giữa thanh nối 1 và 2; 
trọng lực có hướng theo trục y. 
Chọn q;= 9, và q,= 9; là các biến 
khớp tổng quát. Ký hiệu động năng Hình 4.1. Cấu trúc robot 2 thanh nối. 
và thế năng của thanh nối ¡ là K, và P, (¡ = 1, 2). 

Đối với thanh nối 1, sử dụng (4-11), động năng được tính như sau: 


kẻ. 
2 





lˆ 


A1 2 
d)ÖŸ + 5 J6 (4-12) 
Thế năng thanh nối 1 được xác định theo: 
Pị, =m,gl,, sin 6, (4-13) 
trong đó: g - gia tốc trọng trường; 
Ồ, - tốc độ góc của khớp I. 


Đối với thanh nối 2, toạ độ của tâm khối được xác định theo phương trình 
Sau: 


X; =l¡cosØ, +],; cos(8, +9,} (4-14) 

y; =l,sin8, +l,; sin(8, +9, ) (4-15) 
Hai thành phần của tốc độ tâm khối trên 2 trục là: 

&; =¬l sin®;ð; ~ ,; sin(9, +6; Xô, + ö;} (4-16) 

ÿ; =l,cos6,Ô, +1,, cos(6, +6; Xô, +6, ) (4-17) 


Từ (4-16) và (4-17), bình phương tốc.độ dài của tâm khối thanh nối 2 là: 
Vị = X2 tỷ; 
" ....x. cội chui 
vị =lÖôƒ +1? (6, +6, ƒ +2M1,;cos6,(ð?+ôjô,) — (4-18) 
Do đó động năng của tâm khối thanh nối 2 là: 
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K,= 2m,vj +2 1,6, +ô,Ÿ 


m, lô? +1š(ê, +6,Ÿ +2l1,„ eos6,(ð? + 6,ô lb Ị n6 +Ö,Ƒ (4-19) 
Thế năng của thanh nối 2 tính theo: 
P; = m;g|I, sỉn 6, + I,; sin(, +6, )| (4-20) 


Thay động năng và thế năng của các thanh nối (4-12), (4-19) và (4-13), 
(4-20) vào hàm Laprange (4-9) ta được 


b 
2 


L=zm,l2i6ƒ +2 116} 
Ì 2 ] 
+ šmj|lỶ 6ƒ +122|, +ô;Ƒ + 2012; eosð; (6ƒ + ¡ô ;)|+z!-É 6, +6;Ÿ 
~ mịgÌ,¡ sin Ôụ —matll sinÔi +Ì;; sin(9, +6) | (4-21) 


Lấy đạo hàm hàm Lagrange (4-21) lần lượt theo các biến ôi. 8; và sau 
một số phép biến đổi ta được: 


ạ FIEi [mlậi +1, + m;( + l8; +20 ø› e056,}+J, b, 


dt\ 0 (4-22) 
+ [m;(; +11; cos6; Ì+ 1; B: —=m;l, l,;- sin Ô ;bô, 6,+ 6) 
ấ_ =—mygl„ cosÖ, - m;g|l,cos9, + tạ„ cos(®, + 6; )} (4-23) 
; 


Tương tự, đạo hàm hàm Lagrange (4-21) lần lượt theo các biến ð;, 9, của 
thanh nối 2, ta nhận được: 


FIEx = lm;(b, +; cos8;)+ 1;B, +Ím;l, + 1, , —m;Ì,l,; sin Ô;Õ,Ô; 
) 


(4-24) 
ðL 


m7 —m;gl,; cos(9, + 6; )~ m;Ïl„; É; +ô,ô; )sin Ỗ, (4-25) 
Mễ 


Mômen của khớp 1 và 2 được tính theo (4-10) với sử dụng (4-22) và (4-25): 
M,= [mlệ, +; +m;(ˆ + là; +211,a cos6; }+ 1; Bị 
+[m;(Ệ; + hl„; eos6, +1, B; -mạl,l,; sin6,(B6,ð, +6} — (4-26 
+ mịgÏ„¡ cosÖ, + m;g|l, cos6, +l,; cos(0, + 9;)| 
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M¿ =|m;ÍÊ; + ll¿; cos6;}+9 Bị + {malỆ2 +J; B; + mạI,1,; sin6;67 


+m¿;øl;2 cos(Ô; +Ð;) (4—27) 
Mômen của 2 khớp 1 và 2 được viết lại ở đạng rút gọn như sau: 

M, = H,jỗ, + Hj;ổ; +hị;;Ô) +2h,¡;Ö,Ô; + g, (4-28) 

M¡ =H;,jỗ, + H„yổ; + N. +8; (4-29) 

trong đó: Hạ =mjll¡ +], + m;( +12 +21; cos6, }+ J; (4-29a) 
Hạ =H„¡ =m;(Ê, + LI,;eos9; }+ J; (4-29) 

Hạ;y= mạ; +; (4-29c) 

hụyạ =hy¿ =-hạy = —m;k1,2 sinÐ; (4-29d}) 

¡ = mịgl„¡ coSỐ, +ø|Ì cos9; +l cos(6, +9;)| (4-2%) 

g; =m›gl,; cos(0, + 9;) (4-29Đ 


(4-28) và (4-29) là các phương trình động lực học của cơ cấu robot 2 thanh 
nối. Phương trình động lực học robot 2 thanh nối có thể được viết ở dạng 
phương trình ma trận sau: 


M =H@)q + V(4,q) + G(q) (4-30a) 
trong đó: 
Hị H; |, hị;;Ð3 + 2h¡¡;Õ,Ô; |, Bị 
Hrn li ‡**Ì 122⁄2 Số ?2|:G= 

2L 222 hạ¡¡Ôi 5; 
Hoặc ở dạng: M = H(q)8+ C(,q)q +G(4) (4-30b) 
Với: C ng hịyÕ; Tài 

hạnÔ, 0 


4.3.2. Phương trình động lực học dạng đầy đủ 
Để phân tích ý nghĩa các thành phần mômen của phương trình động lực 
học ta viết phương trình (4-28) và (4-29) ở dạng đầy đủ như sau: 
M, = Huỗ, + Hị;ỗ; + LG + hị„;6? + hịi;Õ,Ô; + hị;¡Ô;Ô, tẾối (4-31) 
M;= H;jÕ, + H„;õ; + hô} + h;„;Ö} + h;¡;Ô,Ô; + h„;¡Ô;Ô, +8; (4-32) 


Đối với phương trình động lực học (4-28) và (4-29) của robot 2 thanh nối 
(hình 4.1): hạ = 0; hại; = 0; hạ;, = 0; hạạy= 0. 
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Xét trường hợp tổng quát cho các khớp i, j, k cúa robot, ý nghĩa các thành 
phần của phương trình động lực học được giải thích như sau: 

+ Thành phân H,Ö, là mômen ở khớp ¡ gây ra bởi gia tốc của khớp ¡ với H, 
là mômen quán tính hiệu quả của khớp ¡. 

+ Thành phần H,ổ; và H;ỗ, tương ứng là mômen ở khớp ¡ Và j gây ra bởi 
gia tốc của khớp j và ¡; H, và H¡ là mômen quán tính đối ngẫu giữa khớp ¡ và 
khớp j hoặc j và ¡. 

+ Thành phần h,;Öj là mômen hướng tâm tác dụng lên khớp ¡ gây ra bởi 
tốc độ của khớp j. 

+ Thành phần h„ô,ô, + h,„ ð,Ö, là lực nhớt tác dụng lên khớp ¡ do tốc độ 
của khớp j và khớp k gây ra. 

+ g, là thành phần trọng lực của khớp ¡. 

4.3.3. Hệ phương trình trạng thái động lực học 

Trong nhiều trường hợp nghiên cứu, thiết kế hệ thống điều khiển robot, 
phương trình động lực học thường được viết ở dạng phương trình trạng thái. 

Hệ phương trình động lực học Lagrange của robot 2 thanh nối (4-28) và 
(4-29) có thể viết ở dạng ma trận như sau: 


Mh tị bì sài li (4-32) 
M; Hạ H;; |6, ¬hôỷ 5; 


Các thành phần của ma trận H (H,,, H;, H;,, H;,) được tính theo (4-29a) + 
(4-29c). 


"h=-m¿ijl,;S; với S; là ký hiệu của sinÔ;. 
Đặt biến trạng thái cho từng khớp I và 2 là: 


X, "[|"] (4-33) 
Xa 9, 


c 8 
Ñ, TP | “li: (4-34) 
X;¿ ? 


và tín hiệu vào là các thành phần mômen của các khớp tương ứng: 


- uì M, 
Ũ = nI (4-35) 
2 2 


Khi đó phương trình (4-32) được viết lại ở dạng sau: 
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lim ni Xi Re + 2 RM là] (4-36) 
u;J |Hạ, Hạ; |x;, — hxj; Ê:› 

Sử dụng ký hiệu biến trạng thái (4-33) và (4-34) và tín hiệu vào (4-35), có 
thể viết hệ phương trình vi phân trạng thái của khớp I và 2 như sau: 


Khóp I: l Đà (4-37) 
_ l&¡; =f(X.)+HU 
Xại =X;; 
Khớp 2: 4-38) 
T La : 
Trong đó Hˆ là ma trận nghịch đảo của ma trận H: 
Hạ; s ni 
Hạ, H -H H 
H=| "| và HÀ = 21 "1 (4-39) 
Pˆ Hạ; Đụ 


Các phương trình thứ nhất của (4-37) và (4-38) đã được xác định. Các 
phương trình thứ hai có thể xác định được nhờ giải hệ phương trình (4-36) và sử 
dụng (4-33)-(4-35): 


'| Hạ - Ị mn Ì ị Hạ; ni] ] 
li -Hy Hy Hy) §› # Hạ, H, |[u; 
Xạ; Dụ Dự 
(4-40) 
trong đó: Dự = HụH;; - Hệ. 

Xị; và X;; được xác định từ (4-40). Kết hợp với các phương trình (4-37) 
(4-38) và (4-40), hệ phương trình trạng thái mô tả động lực học của khớp I và 2 
có dạng như sau: 

Khớp i: 


Xịi =XỊ¿ 


„ 1 Hạạu, -H¡;u 
XI; =~—p_Ha(x$, +2hx,;x„;Ì+ Hị;hx$ +H;;gị — Hịg,|+-201—Hau, k2 
M M 
(4-41) 
Khớp 2: 


Xại =X¿; 


› l H,¡u¡ +H¡;u 

X; =~=—EHụ (hở, +2hx,„x„, Ì~ H,ihx$ +Hị¡g; - Hị;8, = = 
Dụ N 
(4-42) 
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Như vậy với khái niệm hệ thống nhiều đầu vào, nhiều đầu ra (MIMO), 
robot 2 thanh nối có thể mô tả như là một hệ 2 đầu vào 2 đầu ra biểu diễn bằng 
2 hệ thống nhỏ, mối hệ thống nhỏ tương ứng với khớp I và khớp 2. 


4.4. Phương trình động lực học của robot n thanh nối 


Ở phần này sẽ xây dựng phương trình động lực học Lagrange của robot n 
thanh nối. Quá trình tiến hành theo 5 bước: 

+ Tính tốc độ của điểm bất kỳ trên thanh nối; 

+ Tính động năng; 

+ Tính thế năng; 

+ Tính hàm Lagrange; 

+ Tính mômen và lực của các khớp. 

a. Với robot n thanh nối, trên mỗi thanh nối gắn một hệ trục toạ độ trong 
đó mối quan hệ giữa hệ toạ độ thanh nối ¡ và i-1 biểu diễn bằng phép biến đổi 
toạ độ “"T, Do đó phép biến đổi toạ độ từ thanh nối ¡ đến hệ toạ độ gốc là °T; 
(xem hình 4.2): 

ST TT TU (4-43) 

Trong đó phép biến đổi '"T; là hàm của biến q, của khớp ¡. 

Xét một điểm bất kỳ trên thanh nối ¡ và biểu diễn nó trong hệ toa độ thanh 
nối ¡ bằng vectơ '7 


T= [X;. Vị. #; nIÍ” 
Vị trí của điểm đó trong hệ toạ độ gốc biểu diễn bằng quan hệ: 

°T=”T,'Ƒ (4-44) 
Vì thanh nối là vật thể cứng, nên quan hệ '? trong thanh nối ¡ là hằng số, 

tức là: 

dĩ 

—=0 4-45 

a (4-45) 
Lấy đạo hàm phương trình (4-44) và sử nh (4-45) ta có phương trình sau: 


d°T Kinh hô°T; 
. sỹ F=-2(°T,)'T+ NA _ na" (4-46a) 


Ghi chú: Do T; = f(q,, q›,...., q,) không phụ thuộc vào q, (j > Ì). 
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Hình 4.2. Mô tả điểm bất kỳ ở thanh nối ¡. 


Ũ 


T, : 
Nên sh =00>]) tức làj=i+ 1,..., n), nên thay chỉ số giới hạn ¡ bằng 





j 


n. Do đó (4-46a) có thể viết thành: 


= ¿ 
=° 4-46b 
HA" bu“ 





J=l 


Sử dụng khái niệm vết của ma trận (Trace(A), bình phương tốc độ của một 
điểm bất kỳ trong thanh nối ¡ được tính như sau: 


T 
Š ° " ã°T. ô°T, 
®ƑT%Ƒ =Trace\°7°rT ]= Trace| "| ——tá.!T im ácT] (4-47) 
( }- Ỳ P ôq, 3 > ôq F 





Sau một số phép biến đổi ta có: 
. PT. uy ĐT, 
°TT%= Trac GÌ đ;d (4-48) 
> ồq; ôq, : 
b. Động năng của một điểm bất kỳ trong thanh nối ¡ có khối lượng dm và ở 
vị trí 'T được tính bằng: 


đK, =L°F*°iám = Ì| *Ÿ*Tred Ê“Tuppr Ê TỶ Tay lim — (+49) 
‡ 2 2| 4 ôq; âqy 4,4: 
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Đo đó động năng của thanh nối được tính bằng tích phân theo toàn bộ 
thanh nối ¡: 





S+rT 
= ÍdK, =s Tae há ['ffdm Bi đjậy (4-50) 
thanh ì j=l k=l m thanh ¡ ôq, 


Ký hiệu J,= ['F? dm - Ma trận quán tính giả. 


thanh ¡ 

[êm » t,ảm Ịx r,dm J'sảm 

ƒx rudm ƒs dm ƒs r.dm [ám 

Tnhêm Íuhảm hiếm [sản . 


T rudm ƒ rudm [Ísảm Jam 


Các phần tử của ], được xác định như sau: 
lị„ = llỆ rˆ+!r2 im 


thanh ¡ 

1„= [Êr ¿r,}im (4-52) 
thanh ¡ 

mX, = Ỉ 'rdm 
thanh ¡ 


Các thành phần I,.„ I„„. Ï„„ I,„„ m,Y;, m;Z;, xác định tương tự như ], 


ixx? lzy 
hoặc m,X,. 
Khi sử dụng các biểu thức: 
['sam==2 [Cj+2)đm +2 [Ct+t2đm +2 [C2 +ig Mm 
CH¿„ + lừy +l„) 
Sẽ 
ƒh rẻm =2 [tr rậ+Ìr? ?)dm —2 [tr + 2)dm +2 Crẻ+Ìr2)»dm 
= đqụx — Ty + lz;) 
2 
[tem =2 [C+2 ám + 3 [Crt+h2)dm =3 [trả nám 
(4-53) 
— địy + lựưy ~l¿;) 
x 2 
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ma trận quán tính }, được viết lại như sau: 
— ID + ly + lự„ 


2 lừy lị„ mịX; 
lyy _ 1y + = 
J;= lúy 2 lự„ mỹi | (4-54) 
1y +l¿„ —Í 
lyy lự„ —————= m¿Z, 
2 
m,%; m,Yy; m,Z, m, 


trong đó: 'Ì7= [ PD.. Ủy tr1Ƒ - vectơ vị trí của phần tử ¡; 


X¡,Y¡„, - các thành phần của vectơ tâm khối trong khung toạ độ thanh ¡. 
Tổng động năng của robot được tính như sau: 


n n1? ST 
K =>K, z3 Tre TỪ nỘ cái 0 đán] (4-55) 














i=l j=l k=i Q; cô. 
Sử dụng tính chất của Trace (PL...) (4-55) được viết thành: 
= _ `: 1 . —.— 
K =mme Ÿ .. (4-56) 
J}=l k=l 
trong đó thành phần ma trận h, được tính theo: 
ò 8°T,, ô°TT, 
H„ụ => Trace *j, (4-57) 
N È ôn; ôq, | 
ô?T. 
Do ———=0 (j>i¡) nên có thể viết (4-57) thành: 
j 
H1 ơ s+T 
` Tmọ| hi 2T (4-58) 
i=max(j.k) ôn; ôqy 


Do h„ = h,, nên ma trận vuông H(q) là ma trận đối xứng, đồng thời động 
năng là một đại lượng dương với ả z0, nên các thành phần của H(q) là các số 
đương. 


c. Thế năng của thanh nối thứ ¡ được xác định theo biểu thức: 


P, ==m;g”°T,'f (4-59) 


trong đó: g= kg, ,8›g,.lJ - Vectơ gia tốc trọng trường; 
m, - khối lượng của thanh nối; 


'T_- vectơ vị trí thanh nối ¡ trong hệ toạ độ của thanh nối ì. 
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Tổng thế năng của hệ thống là: 

P =-Ÿ m,gT°*T,'? (4-60) 
t=Í 
Chú ý rằng mịr là cột thứ tư của ma trận J, nên có thể viết tổng thế năng 
như sau: P= _ g"°T,(M;e, (4-61) 
Với e„ = [0 Ö o1 

d. Hàm Lagrange của robot là: 

L= XS -P) (4-62) 
í=L 
L(,8) = K(G.8)~P(4) =2 ãT H434 — P(4) (4-63) 
Thay các biểu thức tính K (4-56) và P (4-60) vào (4-62), sau một số phép 
biến đổi ta nhận được hàm Lagrange có dạng sau: 

Le2 ŠŠ Š Tác “am Thái + mg” °T? (4-64) 


e. Tính các thành phần của phương trình động lực học: 


ôL, ôK 
—=—=H 4-65 
ø 88 (3q (4-65) 
d ØL 
—=—=[ H 4-66 
ai để (q8 - Hq)q (4-66) 
ôL _ 1 Ê lạ ¬ruye=+]_ ÔP() 
= 4-67 
Ai” xanh Ha -lc đe 
Do đó phương trình động lực học robot có dạng: 
—t  x„nm lỡØEr,_-]l ôP(4) 
H H(qg)q-—_——|q H —=M 4-68 
(Đã+ ñq)ã~> mủ qä|+ n (4-68) 
› — x— Lô =—TryờỶïaGc „x ØK 
Ký hiệu: V(q,qg)=H -—lq H =Hq~-—— 4-69 
ý hiệu: V(,g) = H@)ä zah (ä]= Hả n (4-69) 
- thành phần mômen nhớt và hướng tâm 
G(q)= — - thành phần mômen trọng lực. (4-70) 


Khi đó phương trình động lực học có dạng như sau: 
H()8 + V(q,4) + G(4)=M (4-7) 
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Tính các biểu thức (4-64) + (4-70) sẽ nhận được các biểu thức sau: 
Mômen của khớp ¡ được xác định theo (4-10) với hàm Lagrange tính theo: 




















M; = =SHui +2,23Dụ,đ;dy +D, (4-72) 
ƑEL kzIl 
a ðT,. 8917 
trong đó: Hạ= tme\ t1, tj (,j = 12.....n) (4-72a) 
p=nax(,j) ôn, ° ân, 
T; 8°T. 
v,(q,đ)= Trace J đả; đ,J,k=1,2,...,n) 
II S l» ôn, m hà 
(4-72b) 
a! T, 2°T7 
Ủ„ ® Trac Tóc (4-72c) 
_ lộ: la : ôq; 
8¡ ==Ö,m,g' (4-72d) 
pel J 


Các phương trình (4-68) + (4-72) có cùng dạng với phương trình động lực 
học nhận được ở mục 2 của 4.3. Trong đó: H(q) là ma trận vuông đối xứng 
(nxn) biểu thị ma trận mômen quán tính với phần tử H(j) xác định theo 
(4-72a), V(q,8) là vectơ (nx1) biểu thị thành phần lực hướng tâm và lực nhớt 
và G(q) là vectơ (nx1) biểu thị mômen trọng lực. Các thành phần mômen quán 
tính và trọng lực có ảnh hưởng lớn đến độ ổn định và độ chính xác vị trí của 


robot; Các lực hướng tâm và nhớt chỉ có ảnh hưởng lớn khi rôbot di chuyển với 
tốc độ cao. 


Thành phần V có thể viết tách thành: 
V(4,8) = C(q,ä)4 
Do đó phương trình động lực học được viết ở dạng: 
H(q)ã + C(,4)q + G)=M (4-73) 
Thực tế trong cơ cấu robot tồn tsị lực ma sát nhớt và lực ma sát nh nên 
một cách tổng quát, phương trình động lực học robot có dạng sau: 
H(q)8 + C(.4)đ + G(4) + F,4 + E,(4) +M, =M 
trong đó: F, - ma trận hệ số ma sát nhớt; 
F;(đ) - vectơ lực ma sát tĩnh; 
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Mẹ - vectơ mômen nhiễu gây ra bởi mô hình động lực học không 
chính xác. 
Nếu kể cả ngoại lực tác động vào tay robot, mômen khớp phải được cộng 
thêm thành phần mômen gây ra bởi lực tác dụng lên tay (4-102): 
H()8 + C(,4)8 + G(4) + F„q+ E,()+ Mạ =M+Ñ, 
Ví ấy 4.1: Sử dụng phương trình động lực dạng tổng quát để xây dựng 
phương trình động lực học của robot 2 thanh nối ở hình 4.1. 
Với cấu hình robot 2 thanh nối ở hình 4.1, áp dụng phương pháp đặt khung 
toạ độ D-H, nhận được ma trận A; và A; như sau: 


G,.=§,: 06: Eyc2-S,. (8đ, 
A,=|Š Có 0 b§, A,-|% €9 hộ; 
0 0 1 0 0 0 1 0 
0 0 0 1 0 0 0 1 


Ma trận biểu điễn quan hệ khung toạ độ thanh nối 1 và 2 so với khung toạ 
độ gốc tương ứng là: 


€¡ —5¡ 0 lịC, Cạ¿ -§; 0 l¡C¡ +lạC¡; 
S C 9 1S S C 0 IS, +lsS 
0 ¡ Cị lồi ọ ¿ Ca IŠi tlạ§12 
= đà 4-74 
NÌg. nay co |5 2# ¿1 0 (hỜ) 
0 0 0 1 0 0 0 I 


Tính các đạo hàm riêng của ma trận biến đổi “T, và °T; tương ứng so với 
các biến khớp 1, 2: 











-§¡, -C, 0 -l§, 
ôT, ôT, |C, -§; 0 lC, (474 
ôq _ 8, 0 0 1 0 
0 0 0 1 
0 
S hcÔ (4-75b) 
®; 
=Šn ~G; -l§, -l;5„; 
= = (4-75c) 








0 

ð%T, _ô'T, |C; -§%;, 0 lC¿+l;Cụ 
1 
0 
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-Ñg; C; 0 ~l;5Š; 
ốT, z ô°T, E €; ¬—§, 0 1C, (4-75đ) 
q; Ø9; 0 0 1 0 
0 00 Ị 
Sử dụng (4-72a), tính H; , j = 1, 2): 
2 ô°T ô°?TT omT ° °o+T 
Hị =3” Trac “1y ——— |=Trac Ni _ Ì- cao Ê IS 
p=I ôq ` ôn; n â; &, ˆ ân, 
(4-76a) 
2 Ô°T, TP, °TT 
Hạ; =Ð`Trac nộ = me _ mi hề T | (4-76b) 
pz2 ồn; ?° ôn 
H S94 TE = Tae nộ (4-76c) 
„ p=2 ôq; b s h ôq; 
2 ô°T, ` ° o+T 
Hạ; =3 Trac — me S AI nề kỆ (44-764) 
p=2 ôq; ° ôn, 


Mômen quán tính giả của khớp I1, 2 được xác định bằng sử dụng (4-52) + 
(4-54): 


am, 0 0 -am, am, 0 0 ~a,m, 
0 
II 0 0 0 1e 0 0 0 0 (4-7?) 
0 00 0 0 0 0 
-=am, 0 0 m, -am;y 0 0 m; 


trong đó: a; = Ì, - I, - khoảng cách từ tam khối thanh 1 đến gốc khung toạ độ 
thanh 1; 

8; = Ì,- l; - khoảng cách từ tâm khối thanh 2 đến gốc khung toạ độ 
thanh 2. 

Theo (4-76), sử dụng (4-74); (4-75) và (4-77) lần lượt tính được H¡,, H,;, 


H;, và H,;: 
° 2rT 0 °srT 
TT, 881; c T b, ô°Tj 
ch Øqi ồq; ôn 


=J¡ +lm; + (Ệ + l3 +2ljl2C;)m; +J, 


Hị; =Trac 

















(4-78a) 
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ĐT) Ø1, 
Hạ ~Tre| P 21, `. th +1,1C;)m; +J; (4-78b) 











q› Øq\ 
ô°T. ô°TT 
. -Tas| âu Ì-&: + hlzC;)m; +1; (4-78c) 
2 1 
ô°T;, ô°T; 
Hạ — Tae TU, ân : ]*m: +; (4-78d) 
2 2 


Các thành phần D,,,, D,;; được xác định theo (4-72c) với sử dụng (4-74), 
(4-75) và (4-77): 


30 0T 30 0T 
T T, 
Đị, =mno|Ê ÓC ÁI t HS }-o (4-79a) 
































(ôa,)” ` ÔN CC T 
ô?°T a?TT 
Dụ; HE J; PT =-m;kl,;S; (4-7090) 
Tương tự, có thể tính được các hệ số D¡;; và Dạ, 
Dị = -m;1,1Lz 5% và Dạ¡;= m,1,1 z5; (4-80) 
Sử dụng (4-72d), các thành phần ø; và g; được tính như sau: 
~ổ Trị ô”T;;_ 
Ø =—m;g` â, T _m;g" . =m,l,¡sC, + m;g(C, + l,;C¡;) 
(4-81a) 
-~'0- Teo ô°T;;_ 
g:=-mỹ" n lr —m;g' _ =m;gl,;C¡; (4-81b) 


trong đó: ø§” =[0,—g,0,1]Ÿ ; 
',?7 - tương ứng là vectơ vị trí điểm trọng tâm của thanh 1, 2 trong 
khung toạ độ thanh nối 1, 2. 
!r=[-a,,0,0,1]7 và ?? =[~a;,0,6/1] 


Vậy phương trình động lực học của robot 2 thanh nối được viết ở dạng 
(4-72): 


M, = Hujỗ, + Hồ, + Di ô)Ô, + D,;6,Ô; + Dị;¡Ô;Ô, + D,;;ô) +ø. - (4-82a) 
M¿ =H;¡ổ, + H„;Õ; + D„¡;Ô,Ô, + Dạ¡;Õ,Ô; + D;;¡ô;Ö, +Dz;Ðj+pg; (4-82b) 


Với: H, Hị;, Hạ,, Hạ; xác định theo (4-78a) mơ (4-78đ); 
Dụy=Ô0; 
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Dị = Dụ; = Dụ, = -D¿,, tính theo (4-7290) và (4-80); 
Dạu = Dạy = Dạ;; = 0; 
B¡. 8; tính theo (4-81a) và (4-81b). 


4.5. Hệ phương trình trạng thái động lực học của robot n 
thanh nối 


Để viết phương trình động lực học ở đạng phương trình trạng thái, ta định 
nghĩa các vectơ sau: 
a. Vectơ trạng thái bậc n cho hệ thống: 
X.NWÌ='S, 2X. Đau Tế (4-83) 
trong đó X, =[xạ,x;;]" =[q,,4,]Ï - là veetơ trạng thái của thanh ¡. 
b. Vectơ đầu vào bậc n: ï 
U=[U,,u;,.....u„}` =[M;,M;„..,M, Ƒ (4-84) 
trong đó u¡ =M, - là mômen đặt vào cơ cấu chấp hành của thanh thứ i. 
Phương trình động lực học của robot (4-71) có thể viết dưới đạng sau: 
Hị,.... Hị, M Vị &n M, 
"3... =.Ắ.... (4-85) 
H„; . Hạ đạ Vụ Bà M 
Sử dụng ký hiệu vectơ (4-83) và (4-84), phương trình (4-71) có thể viết ở 
dạng phương trình trạng thái mô tả robot như sau: 
X@)=fŒX(),U() (4-86) 
ở đây: f (.) là hàm vectơ phi tuyến ( 2n x 1 ) và khả vi liên tục. 


Khi đó nhận được n hệ phương trình trạng thái theo n khớp, trong đó 
phương trình trạng thái thứ nhất của mỗi khớp là: 


Xi =X 
Xại =X;; 

+ (4-87) 
Xai Xn2 


11? 


Phương trình thứ hai có thể được xác định bằng cách giải phương trình sau: 
XI; ViXii,Xi;) Ei(XI;) Mì 
H} : |+ : + : =| : (4-88) 


Xa Vn(Xjts Xa) En(Xmi) tạ 


Vì ma trận quán tính H là không đơn nhất nên có thể biểu diễn các vectơ 
trạng thái như sau: 


XỊ; MỊ VviXu,Xi;) giŒ¡¡) 
: |=H | : |-H" +| (4-89) 
Xn¿ Mạ Va(Xm› Xaz} 8a(Xạn) 


Hoặc (4-89) có thể viết như sau: 


XI; ft) KỦ k Đụ tị 
;: I=} : |+ _ . ( : (4-90) 
Xụa f.@®) bại . bạn tạ 


Từ đó hệ phương trình trạng thái cho khớp ¡ là: 
Xi =Xị 
RÌ ta (4-91) 
k, =fi;()+b,;(X)U@) 


$ 


trong đó: f;;(.) là thành phần thứ ¡ của ma trận —H"1 ()(Vq, ẩ)+g &)) 
b,;X) là hàng thứ ¡ của ma trận H'(8). 


4.6. Động lực học của robot với cơ cấu chấp hành 


4.6.1. Động lực học của robot với cơ cấu chấp hành bậc 2 


Hệ thống chuyển động robot gồm cơ cấu cơ khí và hệ thống truyền động. 
Có ba dạng hệ thống truyền động khớp robot: Hệ thống truyền đ“ ¬g điện với 
động cơ điện, hệ thống truyền động thuỷ lực và hệ thống truyền động khí nén, 
Trong mục này sẽ đề cập đến động lực học của hệ thống robot với hệ thống 
truyền động điện sử dụng động cơ một chiều. 

Hệ thống cơ khí thông thường có quán tính lớn so với quán tính hệ thống 
. truyền động điện, do đó ở đây, xét hệ thống truyền động điện động cơ một 
chiều với bỏ qua điện cảm phần ứng động cơ. Sơ đồ nguyên lý và sơ đồ thay thế 
động cơ một chiều ở hình 4.3. 
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+ 
T, ⁄ : 
R 
U 
_ LỆ lo ° Ầ 
E 
a) - 


b) 


Hình 4.3. Sơ đổ động cơ điện một chiều: 
a- Sơ đồ nguyên lý; 
b- Sơ đồ thay thế. 


Phương trình mô tả động cơ một chiều truyền động khớp ¡ có đạng: 


Uợ; =R,l, + K ¿ep, (4-92a) 
Mọi ~M¿ =1g,Ôp¡ + Bạj@p¡ + Mụy (4-92b) 
Mẹ, = K„j, (4-92c) 


trong đó: Ú;, - điện áp phần ứng động cơ truyền động khớp i;. 
1, - dòng điện phần ứng động cơ truyền động khớp ; 
R,- điện trở phần ứng động cơ truyền động khớp i; 
0g; - tốc độ góc động cơ truyền động khớp i; 
K ¿ - hệ số sức điện động; 
Kạ¿ - hệ số mómen; 
Jø, - mômen quán tính động cơ truyền động khớp i; 
B;; - hệ số ma sát nhớt động cơ truyền động khớp i; 
M,,, M,„, M,, - tương ứng là mômen động cơ, mômen cản động cơ và 
mômen không tải khớp ¡. 
Kết.hợp 3 phương trình (4-92a)-(4-92c), sau một số phép biến đổi nhận 
được phương trình: 
KuUb =1p8p + Biáp, + K¿M, + Mụ, (4-93) 
với: qp, - góc quay động cơ thứ i; 


K„= ng tỷ số truyền hộp số; 
đoi 


+ KuKm và K K 


B,=B di 
R, R 


1 Đi 
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Sử dụng khái niệm ma trận đường chéo và vectơ, phương trình vectơ mô tả 
hệ thống các động cơ của các khớp robot được viết đạng sau: 
KuỦy =Jp8p + Bo +K,M+M, (4-94) 
trong đó: M - vectơ mômen khớp; 
Up- vectơ điện áp động cơ truyền động các khớp; 
đo - vectơ vị trí góc quay động cơ truyền động khớp; 
Ku, 1s, B, K, - các ma trận đường chéo (diag(.)); 
Ku = diag(Œ,); Í› = diag(1s,); B = diag(B,), K, = diag(Œ,,) 
Quan hệ vectơ vị trí khớp và vectơ vị trí động cơ biểu diễn ở dạng: 
q=Kjc = 8o=K¿ 493) 


Phương trình động lực học của cơ cấu chuyển động robot có dạng tổng quát 
như (4-73): 


H(q)8 + C(q,d)q + G(4) =M (4-96) 


Thay (4-95) và (4-96) vào (4-94), nhận được phương trình động lực học của 
cơ cấu chuyển động và động cơ điện với đầu ra là vectơ vị trí khớp như sau: 


is + K?H@)R + lb + K?cq,Đh +K,M, +KG()=K,KuỦp (4-97a) 
hoặc với đầu ra là vectơ vị trí động cơ là: 
H p† K?H@ĐRo + lb + K?C@,ðlo +M, +K,G()=KuUp (4-97b) 
Ký hiệu: H)=J; + KH) 
C.g=B +K‡C(,8) 
G'q)=KG() 
K'=K,Kụ 
Phương trình (4-97a) có dạng tổng quát sau: 
H)§ + C'(.,4)ã + GÌ) + K,M, = KỈUs (4-98) 
Phương trình (4-98) có dạng tương tự như phương trình (4-96). Phương 
trình (4-96) mô tả động lực học của hệ thống chuyển động cơ khí của robot với 
M là vectơ mômen khớp. Phương trình (4-98) là phương trình động lực học của 


hệ thống chuyển động cơ khí và động cơ truyền động với Up; là vectơ điện áp 
phần ứng động cơ truyền động các khớp robot. 
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4.6.2. Động lực học của các khớp độc lập 


Dạng tổng quát của phương trình động lực học robot và động cơ truyền 
động có đạng như (4-98): 
H G08 + C'(4.4)8 + G'() +M, = KỈU, 
Phương trình động lực học cho khớp ¡ được viết ở dạng sau: 
MS + K?H; @) |ä: +[B, + K$C¡(,8) |à, +K„M,¡ + KG,) =KKu;Uạ; 
(4-99) 
trong đó: H; - thành phần đường chéo (phần tử (1,i)) của ma trận H; 
C¡ - thành phần đường chéo (phần tử (¡)) của ma trận C; 
G, - thành phần hàng ¡ của ma trận G. 
Bằng viết tách ma trận H; thành hai thành phần: 
Hị =H¿ + AH; 


(4-100a) 
với: H, - thành phần chỉ phụ thuộc vào các tham số hằng số khớp í, ví dụ: 


mômen quán tính, khối lượng khớp i,... 


AH; (4) - thành phần phụ thuộc vào các biến khớp ¡ và tham số và biến 
khớp khác. 
Phương trình (4-99) được viết ở dạng sau; 
[Dpị + K:H¡ lắ; + Bịä; + KgMạ¡ = 


= K„Ku;Up¡ ~ k;[AH¡ä, + C¡(8.8)ả; + G; 4)] 


(4-101) 

Phương trình (4-101) gồm hai thành phần: thành phần tuyến tính chỉ phụ 

thuộc các tham số của khớp ¡; thành phần phi tuyến ràng buộc phụ thuộc vào 

các tham số của khớp ¡ và khớp khác. Thằnh phần ràng buộc được viết ở đạng: 
2 — b2 “ — 2v; = 

k„đ, = k;[AH;q, + C,(q,q)4, +G,(q)] 


(4-102) 





đo›đp›đp 
Hình 4.4. Sơ đồ cấu trúc động cơ - khớp robot. 
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Nếu hộp giám tốc có hệ số k,, nhó, có thê coi thành phân K.¡“d, là thanh 
phần nhiễu của khớp ¡. Điều đó cho phép tách rời độc lập các khớp robot và việc 
thiết kế các bộ điều khiển các khớp robot sẽ đơn giản hơn. Phương trình (4-101) 


được biểu diễn ở sơ đồ cấu trúc như hình 4.4. 


4.7. Lực và mômen fĩnh 


Trong dây chuyển sản xuất, robot có thể làm việc ở trạng thái cân bằng khi 
tay robot mang một vật và không chuyển động. Trong trạng thái làm việc đó, có 
thể coi robot chịu một tác dụng một ngoại lực. Các lực hoặc mômen sinh ra ở 
các khớp cần thiết để giữ robot ở trạng thái cân bằng gọi là các lực hoặc mômen 
tĩnh. Trong mục này trước hết sẽ nghiên cứu mối quan hệ các lực và mômen 
giữa các hệ toạ độ khác nhau và áp dụng cụ thể cho robot. Sau đó sẽ trình bày 
phương pháp xác định lực và mômen tĩnh ở các khớp. 


4.7.1. Phép biến đổi lực và mômen giữa các hệ toạ độ 


Lực và mômen là các đại lượng vectơ được biểu diễn ở các hệ toạ độ 
khác nhau. Ký hiệu vectơ lực là: F¿ = [F,, Eù F,] và vectơ mômen là: 
Em =ÍM,,M,, MỊ]. 

Khi nghiên cứu quan hệ lực và mômen giữa các hệ trục toạ độ, dùng vectơ 
F biểu diễn chung cho cả vectơ lực và mômen trong hệ toạ độ gỐC: 

'F =ŸF,, E„ F„ M,, My, M,]Ÿ (4-103) 
trong đó: F,„ F,, F,— 3 thành phần lực trên 3 trục của hệ toạ độ gốc; 
M,,M.,M, - 3 thành phần mômen trên 3 trục của hệ toạ độ gốc. 

Tương tự, vectơ lực và mômen trong hệ toạ độ C được biểu diễn bằng vectơ 
c, 

Khi có một lực F tác dụng vào vật ở hệ toạ độ gốc sẽ gây ra các lực tương 
đương tác dụng ở các hệ toạ độ khác nhau gắn cứng với vật. Các lực tương 
đương này có thể xác định theo phương pháp “dịch chuyển ảo” được trình bày 
dưới đây. 

Một ngoại lực F tác dụng lên vật gây ra 1 vi phân dịch chuyển ảo ôŠ ở hệ 
tọa độ gốc và lực “F tương đương gây ra một dịch chuyển ảo tương đương 
“2S ở bệ toạ độ C gắn cứng với vật đó. Giả thiết dịch chuyển rất nhỏ không 
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làm thay đổi năng lượng của hệ thống. Khi đó tác dụng của lực E và CF lên 
vật sẽ cân bằng với nhau và bằng: 
FT8S=°ET°§S (4-104) 
trong đó: õS = [dx, dy, đz, õx, õy, õz]T (4-105) 
với dx, dy, dz là các thành phần dịch chuyển tịnh tiến theo 3 trục toạ độ và ồx, 
ỗy, ðz là các thành phần địch chuyển quay theo 3 trục toạ độ. 
Độ dịch chuyển “ðS ở hệ toạ độ C và độ dịch chuyển õS ở hệ toạ độ gốc 
và các độ dịch chuyển đó liên hệ với nhau qua phương trình (3-27 ): 
“äS=°JäS (4-106) 
với °J là ma trận Jacobien đối với khung toạ độ €. 
Thay (4-106) vào (4-104) nhận được: 


FT8S=°FT€J€äS (4-107) 
Quan hệ (4-107) đúng với mọi dịch chuyển ŠS # 0 nên ta có: 
TU TP cJ (4-108) 
hoặc: F=f1T°E (4-109) 


4.7.2. Lực và mômmen của các khớp 


Dựa trên phép biến đổi lực và mômen giữa các hệ toạ độ, ở mục này sẽ xác 
định mối quan hệ giữa các lực và mômen ở hệ toạ độ bàn tay robot và ở các hệ 
tọa độ khớp. 

Xét một robot 6 bậc tự do chịu một ngoại lực '“ F tác dụng ở hệ toạ độ bàn 
tay robot như hình 4.5. Nếu các cơ cấu 
truyền động của các khớp không làm 
việc, tay robot sẽ không chuyển động. 
Khi đó để giữ cho tay robot ở trạng 
thái cân bằng thì các khớp phải sinh ra 
các mômen tương đương cân bằng với 
ngoại lực. 

Công thực hiện bởi lực tác dụng 
lên bàn tay robot sẽ cân bằng với công 
thực hiện đo các lực và mômen ở các 
khớp, nên ta có: Hình 4.5. Cấu trúc robot 6 bậc tự đo. 
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I*ET”%8S = M'õq (4-110) 
trong đó: ”“G - vectơ lực tác dụng ở hệ foạ độ bàn tay robot, gồm 3 thành phần 
lực và 3 thành phần mômen theo 3 trục toa độ: 
TsS - độ dịch chuyển của bàn tay robot 
M - vectơ cột biểu thị mômen của các khớp robot có thành phần là 
lực đối với khớp tịnh tiến và mômen đối với khớp quay. 
öq - vectơ cột biểu thị độ dịch chuyển của các khớp có thành phần là 
õd, đối với khớp tịnh tiến và õ6, đối với khớp quay. 
Độ dịch chuyển của bàn tay robot và các dịch chuyển của các khớp liên hệ 
bằng phương trình (3-27): 
Tế S156 lg (4-111) 
trong đó: "5J - Ma trận Jacobien của robot 
Thay (4-1 11) vào (4-1 10) nhận được phương trình: 


TT đSÊM (4-112) 
Hoặc vectơ mômen khớp được viết ở dạng: 
M=T9JT'*E (4-113) 


Biểu thức (4-113) đúng với robot có số bậc tự do bất kỳ và cho phép tính 
toán mômen cần thiết của các khớp đảm bảo giữ robot ở trạng thái cân bằng 
(không chuyển động) khi biết lực và mômẻn tác dụng vào tay robot. 

Ví dụ 8.2: Robot Stanford có cấu hình như hình 2.22. 

Ma trận Jacobien là: 


20 0 0000 
6 0 L1 900 
rz 0 200000 
0D 1 0000 
0 00000 
-l 0 000 † 


và lực tác dụng lên tay robot là: "“E =[0 0 100 0 -200 1000Ƒ 
Xác định vectơ mômen cần thiết của khớp. 
Giải: 


Áp dụng (4-102) tính được vectơ mômen của các khớp: 
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[M 20 -6 0 0 0 -I[ 0 —1000 
Mỹ; 0 0 2010 I1] 0 2000 
m.|P|-|0 ! 000 0190| | 9 
M,]| |0 0 0 01 0J 09 ~200 
M,| |0 0 0 10 0-2900 0 
M,| |0 0 0 090 01000 1000 

Bài tập 


4.1. Cho robot dạng 2 thanh nối có các tham có cấu hình như hình 4.I và có các 
tham số sau: 
l¿=l; = 1m; l¡ = l; = 0,5 m;¡ mị = mạ = 5Ö kg; Ï, = J; = 10 kg m? 

Góc ban đầu của các khớp robot là Ø, = 0 rad; Ô; = z/6. Các khớp chuyển 
động đều với tốc độ mr/15 rad/s trong thời gian 5 s. Bỏ qua quá trình gia tốc và 
giảm tốc. 

Tính các đại lượng sau cho các khớp ở các thời điểm ban đầu: 2,5 s và 5 s. 
a. Hệ số quán tính và mômen quán tính; 
b. Hệ số hướng tâm và mồmen hướng tâm; 
c. Hệ số nhớt và mômen nhới; 
d. Trọng lực. 
e. Mômen khớp. 
4.2. Cho robot dạng 2 thanh nối như bài I:. 

Góc ban đầu của các khớp robot là Ô, = 0 rad; Ô; = 0 rad. Các khớp chuyển 
động với đồ thị tốc độ như hình 4.6. 

Tính mômen các khớp ở các thời điểm ban đầu; 1,0 s và 2,5; 4,0 và 5 s. 


Ỗ {rad/s) 


z⁄/1Ô 


0 1 4 5 tự) 


Hình 4.6. Đồ thị tốc độ khớp. 
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4.3. Xây dựng hệ phương trình động lực học cho robot 9 — r có cấu hình như 
hình 4.7. 





Hinh 4.7. Cấu hinh robot 6 - r. 


4.4. Cho robot dạng 9 — r chuyển động từ 9 = 0 đến ®/2' Ô = 1/10 rad/s; 

† =0,01 mís; rị = l m;r.¡ = 0,5 m; r, =r— 0,5 m; m, = m; = 10 kg; 
Ji =];= 2 kg m? 

Giá trị ban đầu rạ = 1 m; 

Hãy tính các đại lượng sau ở các góc,0, 1⁄4, 1/2. 
a. Hệ số quán tính và mômen quán tính; 
b. Hệ số hướng tâm và mômen hướng tâm; 
c. Hệ số nhớt và mômen nhớt; 
d. Trọng lực. 
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CHƯƠNG 5 
THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG 


5.1. Khái niệm 


Các chương trước đã trình bày về động học vị trí và động lực học robot. Sử 
dụng các phương trình động học vị trí thuận, có thể xác định vị trí và hướng của 
bàn tay robot tương ứng với các biến khớp; ngược lại từ vị trí và hướng được 
định trước trong không gian, vị trí của các khớp sẽ được xác định nhờ giải bài 
toán động học ngược. Vấn đề thiết kế đường đi và quỹ đạo chuyển động liên 
quan mật thiết đến bài toán điều khiển robot đi chuyển từ vị trí này đến vị trí 
khác trong không gian làm việc. Đường đi và quỹ đạo được thiết kế là các lượng 
đặt cho hệ thống điều khiển vị trí của robot. Do đó độ chính xác của quỹ đạo 
thiết kế sẽ ảnh hưởng đến chất lượng di chuyển của robot trong không gian làm 
việc. 

Đường đi chuyển của robot biểu thị trình tự đi chuyển của cơ cấu robot từ 
điểm này đến điểm khác, ví dụ từ điểm A đến điểm B và tới điểm C (hình 5.1). 
Đường di chuyển đơn thuần là trình tự thay đổi cấu hình robot mà không bao 
hàm yếu tố thời gian. Quỹ đạo chuyển động biểu thị sự di chuyển của cơ cấu 
robot theo thời gian trong đoạn đường di chuyển. Như vậy quỹ đạo chuyển động 
sẽ phụ thuộc vào tốc độ và gia tốc của cơ cấu robot. 


Hình 5.1. Trình tự di chuyển của cơ cấu robot. 


Hai phương pháp mô tả chuyển động robot là mô tả trong không gian khớp 
và không gian tay (không gian làm việc hoặc gọi là không gian Đecac). Phương 
pháp mô tả trong không gian khớp, chuyển động của cơ cấu robot được xác định 
thông qua các giá trị biến khớp. Xét một robot 6 bậc tự do di chuyển từ một 
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điểm A đến điểm B trong không không gian. Sử dụng phương trình động học vị 
trí ngược, có thể xác định được tất cả các dịch chuyển của 6 khớp cần thiết là 
các lượng đặt của các hệ thống điều khiển vị trí các khóp robot. Đặc điểm của 
phương pháp mô tả không gian khớp là có thể đảm bảo được tay robot đạt đến 
vị trí đặt trước một cách chính xác, nhưng đường di chuyển giữa hai điểm không 
thể dự báo được. 

Phương pháp mô tả không gian tay sẽ biểu diễn chuyển động của tay robot 
giữa các điểm bằng các đoạn di chuyển xác định trong không gian tay và được 
biến đổi về không gian khớp tương ứng. Tương tự như trên, xét một robot di 
chuyển từ điểm A đến điểm B, nhưng theo một đường xác định, ví dụ đường 
thắng giữa hai điểm. Để tay robot đi chuyển chính xác từ A đến B theo đường 
thắng (hình 5.2), cần thiết chia đường AB thành nhều đoạn nhỏ và điều khiển 
robot di chuyển bám theo các đoạn nhỏ đó. Phương pháp thực hiện là tính toán 
các tập hợp vị trí khớp tương ứng với các đoạn di chuyển đó và điều khiển khớp 
robot bám theo các vị trí đặt tương ứng của các khớp. Khi tất cả các đoạn di 
chuyển được thực hiện, tay robot sẽ di chuyển từ A đến điểm B theo một đường 
hoàn toàn xác định, 


` 


* » 


Hình 5.2. Trình tự di chuyển của tay robot theo đường thẳng. 


Hai phương pháp mô tả không gian khớp và không gian tay đều được ứng 
dụng trong điều khiển robot công nghiệp. Mỗi phương pháp có những ưu nhược 
điểm riêng. Phương pháp mô tả chuyển động trong không gian tay rất dễ dàng 
quan sát được và đảm bảo cho tay robot di chuyển theo một đường đi xác định. 
Tuy nhiên có nhược điểm là khối lượng tính toán lớn và yêu cầu thời gian xử lý 
tính toán nhanh. Sự đi chuyển của tay robot giữa hai điểm có thể yêu cầu sự 
thay đổi tức thời các giá trị biến khóp như minh hoa trên hình 5.3. 
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&) (bỳ 
Hình 5.3. Trình tự di chuyển của tay robot theo đường thẳng. 


5.2. Cơ sở thiết kế quỹ đạo robot 


Theo nhiệm vụ đi chuyển của robot, quỹ đạo chuyển động của robot gồm 
hai đạng: quỹ đạo điểm - điểm và quỹ đạo qua một số điểm. Quỹ đạo điểm - 
điểm là dạng quỹ đạo chuyển động thường gặp trong công nghiệp bốc dỡ khi 
robot cần đi chuyển từ một điểm này đến điểm khác trong không gian làm việc. 
Trong trường hợp khác, với công việc phức tạp, tay robot cần thiết phải di 
chuyển trong không gian theo một đường bất kỳ qua một số điểm trung gian. 
Dạng quỹ đạo di chuyển, tốc độ và thời gian di chuyển của các khớp robot khác 
nhay tuỳ thuộc vào mục đích và nhiệm vụ làm việc cũng như các điều kiện ràng 
buộc của cơ cấu robot. 

5.2.1. Quỹ đạo điểm - điểm 


Quỹ đạo chuyển động tay robot giữa hai điểm có thể thực hiện với một số 
phương pháp: Tốc độ di chuyển tối đa với thời gian chuyển động của các khớp 
khác nhau; thời gian di chuyển của các khớp như nhau; quỹ đạo di chuyển là 
đường thẳng được chia làm nhiều đoạn nhỏ đều hoặc không đều. 

Xét một cơ cấu robot 2 thanh nối di chuyển từ điểm A đến điểm B như 
mình hoạ trên hình 5.4a; cụ thể cấu hình robot tại điểm A tương ứng với góc 
quay 9,„ và 0;„. cấu hình robot tại điểm B là 6,g và 8;;. Phương pháp thứ nhất là 
thiết kế quỹ đạo giữa hai điểm A và B trong trường hợp cả hai khớp robot di 
chuyển với tốc độ như nhau. Điều đó có nghĩa là thời gian di chuyển của hai 
khớp sẽ khác nhau. Ví dụ trên hình 5.4a, khớp 1 sẽ di chuyển xong trong 2 đoạn 
đầu, khớp 2 sẽ tiếp tục đi chuyển trong 3 đoạn để đạt đến điểm B. 
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a) b) 


Hình 5.4. Quỹ đạo chuyển động trong không gian khớp. 


Ở phương pháp thiết kế quỹ đạo thứ hai, thời gian di chuyển của 2 khớp 
robot như nhau. Để thực hiện điều đó, tốc độ quay hai khớp sẽ khác nhau, dẫn 
đến quỹ đạo hai khớp khác nhau như minh hoạ trên hình 5.4b. 

Trong trường hợp thiết kế quỹ đạo ở không gian tay, ví dụ tay robot cần di 
chuyển theo hình 5.5a. Như phương pháp đã trình bày ở trên, quỹ đạo giữa A và 
B được chia làm một số đoạn nhỏ và xác định được các tập hợp góc quay của 
hai khớp tương ứng với các đoạn nhỏ bằng sử dụng phương trình động học 
ngược. Mặc dù quỹ đạo trong không gian tay là đường thẳng và khoảng cách 
giữa các điểm trong không gian đều nhau, nhưng sự thay đổi góc quay tương 
ứng của khớp không như nhau. Số đoạn chia càng lớn thì độ chính xác dì 
chuyển của tay càng chính xác, nhưng khối lượng tính toán sẽ tăng. 

Để tay robot có thể dị chuyển chính xác theo quỹ đạo đường thẳng đã đặt 
trước tay robot cần phải gia tốc nhanh đến tốc độ mong muốn ngay trong đoạn 
đầu tiên của quỹ đạo chuyển động. Nâng cao độ chính xác di chuyển của tay 
robot thực hiện bằng chia quỹ đạo chuyển động thành các đoạn không bằng như 
nhau với đoạn đầu và đoạn cuối có độ dài nhỏ sao cho tay robot có thể tăng tốc 
độ ở đoạn đầu tiên, chuyển động với tốc độ không đổi trong các đoạn tiếp theo 
và giảm tốc độ trong đoạn cuối cùng khi đạt tới điểm B (hình 5.5b). 

Trường hợp tổng quát, quỹ đạo chuyển động tay robot là đường cong (ví dụ 
bậc hai), quỹ đạo chuyển động được chia thành một số đoạn và các giá trị biến 
khớp được tính từ các vị trí của các đoạn chuyển động tương ứng. 
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Hình 5.5. Chuyển động của tạy robot 2 bậc tự do: 
a- Khoảng cách đều nhau; 
b- Khoảng cách không đều nhau. 


5.2.2. Quỹ đạo là một dường di qua một số điểm trung gian 

Trong nhiều trường hợp thực tế, tay robot cần phải di chuyển chính xác 
hoặc lân cận qua một số điểm trung gian. Giả sử một robot chuyển động từ 
điểm A đến C qua điểm trung gian B như hình 5.6. Tay robot có thể chuyển 
động theo 2 cách: tay robot gia tốc đến điểm A để di chuyển đến B; giảm tốc và 
dừng lại điểm B; sau đó tiếp tục gia tốc di chuyển và dừng tại C. Phương pháp 
này sẽ tạo nên sự chuyển động "không mềm mại" với nhiều lần dừng không cần 
thiết. Ở phương pháp di chuyển thứ hai, tay robot sẽ di chuyển "mêm" qua điểm 
B: không dừng tại điểm B mà chuyển động của tay robot là sự kết hợp của 2 
thành phần tại điểm B để tay robot có thể đạt được điểm B; có thể giảm tốc và 
nếu cần sẽ di chuyển theo một đường hỗn hợp tiếp tục di chuyển đến điểm C. 
Do chuyển động của tay robot là sự phối hợp của nhiều đoạn, tay robot sẽ qua 
một điểm B (khác điểm B) như minh hoạ trên hình 5.6a. Để tay robot qua chính 
xác điểm B, cần xác định điểm BỈ (hình 5.6b) để sau khi phối hợp các chuyển 
động, tay robot sẽ đi qua điểm B. 


ọ : 


b) 





Hình 5.6. Chuyển động của tay robot qua một số điểm trung gian. 
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5.3. Thiệt kế quỹ đạo trong không gian khớp 


Thêng thường quỹ đạo ở dạng đa thức bậc cao sẽ đáp ứng được các yêu cầu 
về vị trí, tốc độ và gia tốc ở mỗi điểm giữa 2 đoạn di chuyển. Ở mục này sẽ 
trình bày phương pháp thiết kế quỹ đạo trong không gian khớp với xác định 
đường biểu điễn của vị trí khớp theo thời gian. 

5.3.1. Quỹ đạo là đa thức bậc 3 

Từ vị trí ban đầu và hướng của tay robot, sử dụng các phương trình động 
học ngược xác định được các giá trị biến.khớp tương ứng. Bài toán thiết kế quỹ 
đạo cho khớp là xác định đường biểu diễn của vị trí khớp (góc quay của khớp 
quay hoặc độ di chuyển của khớp tịnh tiến) theo thời gian khi di chuyển từ vị trí 
ban đầu q, đến vị trí cuối cùng q, trong thời gian t„, với q là biến khớp tổng 
quát. Quỹ đạo di chuyển của khớp giữa hai vị trí sẽ thoả mãn 4 điều kiện: vị trí 
ban đầu và vị trí cuối cùng; tốc độ tại vị trí ban đầu và tại vị trí cuối cùng. Do đó 
đa thức bậc 3 sẽ thích hợp cho quỹ đạo của khớp robot: 

q() =a¿,+ait+a¿tP tayt (5-1) 

Các điều kiện đầu và cuối là: 

q(t;}= q¿ 
q)=q, 
qŒ,)=đ, 
qŒ,) =đ, 

Trong thực tế, thường gặp trường hợp tốc độ tại vị trí ban đầu và tại vị trí 

cuối cùng của khớp bằng không: 
đẹ =0; d,.=0 (5-2b) 
Đạơ hàm phương trình (5-1) nhận được phương trình sau: 


(S-2a) 


4Œ)=aá¡ +2a¿t +3a,U (5-3) 
Sử dụng các điều kiện đầu và cuối (5-2), nhận được 4 phương trình sau: 

q(fg)= ao =qọ 

q(1,) = 8g + AI, + đạt, +ayf =q, (5-4) 

đŒa)=ai =0 


đ(t,)=a, +2a;f, +3a;tƒ =0 
Giải 4 phương trình (5-4) sẽ xác định được 4 hệ số ao, ay, a; và a;. Từ quỹ 
đạo dạng đa thức (5-1) sẽ xác định được vị trí đặt của khớp tại thời điểm bất kỳ. 
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Những giá trị đó là tín hiệu đặt cho bộ điều khiển vị trí để truyền động 
khớp di chuyển đến vị trí tương ứng. Trình tự tính toán trên có thể áp dụng độc 
lập cho các khớp robot. Trong quá trình làm việc, tất cả các khớp robot được di 
chuyển đồng thời từ các vị trí ban đầu tới vị trí cuối cùng của các khớp. 

Khi robot di chuyển quả nhiều điểm, vị trí và tốc độ tại các điểm cuối của 
mỗi đoạn là giá trị ban đầu cho các đoạn tiếp theo. 

Ví dụ 5.1: Thiết kế quỹ đạo chuyển động cho khớp 1 của robot 6 trục từ 
góc ban đầu 30° đến góc cuối cùng 60° trong 5 s. Xác định góc quay của khớp 1 
tại các thời điểm 1 s, 2 s, 3 s, 4 s. 

Sử dụng phương trình (5-4) với thay 9 = q nhận được các phương trình sau: 

0(0)=aạ =30 
(5) =ag +a,5+a;5? +a;5° =60 
Ö(0)=a, =0 
Ô(5)=a, +2a„5 +3a;5? =0 
Giải hệ phương trình trên được các hệ số của đa thức của quỹ đạo: 
a, = 300; a, = 09/s; a; = 3,6%/s?; a, = -0,48°/s° 
Phương trình biểu diễn vị trí, tốc độ và gia tốc của khớp có dạng: 
6(t)=30+3,6t? —0,48t) 
Ö()=7,2t —1,44t 
Ö(t)=7,2 —2,88t 
Từ đó xác định được các góc quay của khớp tại các thời điểm 1s; 2 s; 3 s; 4 s: 
(1) = 33.12”: 0(2) = 40.56”: 0(3) = 49.44”: 0(4) = 56,880 





b —r+~®—=——rhr,hx+ —.3 
| : ì . ` : 
: ¡ _ W trí khop 
Z : b1 ị | 
= | 
#|_—=‡ - TS t2 ị ‡ 
2 : : : : : : l 
| : 1 : 1 : : 
Vi. 005 36. 00.590 NE: 
Di Ni Gian Mớp mm 
độ Si c3 ——————^~_—~——~-——~>——-_—¬ 
0 0 Y' 218 146 2110841hgiiáag '++4'''1⁄4g ! 6 
Thơi gian (s) 


Hình 5.7. Đường biểu diễn vị trí, tốc độ và gia tốc khớp 1 của ví dụ 5.1. 
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Đường biểu diền vị trí, tốc độ và gia tốc khớp 1 được biểu diễn trên hình 
5.7. Dễ dàng tính được gia tốc khớp tại thời điểm ban đầu là 7,2 9/s? và tại thời 
điểm cuối cùng là -7,20/sÊ. 


Ví dụ 5.2: Giả sử robot đã xét ở ví dụ 5.1 tiếp tục chuyển động tới vị trí mới 
tương ứng với góc quay khớp I là 80” sau thời gian 4 s. Xây dựng đường biểu 
diễn vị trí, tốc độ và gia tốc của khớp. 

Sử dụng kết quả ví dụ 5.1, vị trí và tốc độ của khớp tại thời điểm 5 s là các 
giá trị ban đầu của đoạn chuyển động tiếp theo: 

6(5)=60” Ô(5)=0°/s 
Sử dụng các phương trình (5-4) cho đoạn chuyển động robot trong thời gian 
4s: 

6(0)=ay =60 
6(4)=ao +a,4+a;4? +a, 4) =80 
Ö(0)=a, =0 
Ö(4)=a, +2a;4+3a;4? =0 

Giải hệ phương trình trên nhận được: 

aa = 609; a1 = 0 '/s; a; = 3,75 s2; a,= 0,625 9/8” 

Phương trình biểu diễn vị trí, tốc độ và gia tốc khớp có đạng: 
(t) =60+3,75t? —0,625tẺ 
Ö()=7,5t —1,8750 
Ô()=7,5 —3,7%t 

Kết hợp các kết quả ví đụ 5.1, đồ thị biểu diễn vị trí, tốc độ và gia tốc của 
khớp 1 robot chuyển động trong hai khoảng thời gian 5 s và 4 s minh hoạ trên 
hình 5.8. 

Từ đồ thị hình 5.8 nhận thấy rằng, mặc dù tốc độ di chuyển của khớp robot 
là liên tục, nhưng gia tốc khớp robot thay đổi tức thời tại thời điển trung gian. 
Trong thực tế, cân phải xem xét liệu robot có khả năng đạt được sự thay nhanh 
gia tốc như vậy không. Với điểu kiện gia tốc robot bị giới hạn ở giá trị cho 
phép, có thể tính được thời gian cân thiết để khớp robot đạt đến được điểm đích. 

Mặt khác, trong thực tế, tốc độ đi chuyển của khớp tại các điểm trung gian 
không cần thiết phải giảm đến không mà có thể được lựa chọn bằng tốc độ của 
đoạn chuyển động trước đó. 
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Hình 5.8. Đường biểu diễn vị trí, tốc độ và gia tốc khớp 1 của ví dụ 5.2. 


5.3.2. Quỹ đạo dạng đa thức bậc 5 
Trong 5.3.1 đã xây dựng quỹ đạo dạng đa thức bậc 3 thoả mãn các điều 
kiện biên về vị trí và tốc độ ở các đoạn chuyển động. Để xét đến các điều kiện 
đầu và cuối về gia tốc chuyển động, sử dụng quỹ đạo dạng đa thức bậc 5: 
q()=ao +a¡t +a;t?+a;tP +a,tt+ayE 
đ()=ai +2a;t +3a;t? +4a,t) + 5a,tẺ (5-5) 
đ()=2a; +6a;t+12a„t? +20a,tẺ 
Các phương trình (5-5) cho phép xác định 6 hệ số thoả mãn 6 điều kiện đâu 
và cuối của vị trí, tốc độ và gia tốc của các đoạn chuyển động của khớp robot. 
5.3.3. Quỹ đạo dạng 2-1-2 
Dạng quỹ đạo chuyển động được sử dụng cho robot công nghiệp là đồ thị 
dạng 2-1-2. Đồ thị tốc độ khớp có dạng hình thang với giả thiết tốc độ ban đầu 
và cuối cùng bằng không, gia tốc chuyển động của khớp là hằng số ở giai đoạn 
khởi động và hãm được trình bày trên hình 5.9. Để đảm bảo tính đối xứng ở quỹ 
đạo, điểm trung bình được chọn: 


=%c†% và tự =k 
2 2 


Quỹ đạo phải thoả mãn các điều ràng buộc: vị trí khớp biến đổi tới qụ tới q. 
trong thời gian t.. Tốc độ khớp cuối đoạn parabol bằng tốc độ không đổi của 


đm 
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đoạn tuyến tính: 





q— 
5. ý KH 
th G-6 | | P— 
Phương trình biểu diễn đoạn khởi `" | 1 | 
động (0-t,) có dạng parabol là: q | | 
(x2 sp Ré “Co Vui 2i | 
q(†)=qo tà n th ^ 
đá“ ==————— 
Do đó vị trí của khớp tại t, là: Ị 
ịỊ 
li ị 
qiŒ¡)= đo +sä (5-7) | 
¿ ——D 
Kết hợp (5-6) và (57) nhận được. L....,.... 7.) 
phương trình sau: 
qịt -¡tct +qc —dqo =0 (5-8) 
Giải (5-8), khoảng thời gian t, 
được xác định theo biểu thức sau: 








2 — —— 
tị = `: tcqị 4, qa) (5-9) 
2 2 \ q 


Từ (5-9) gia tốc của khớp trong 
giai đoạn khởi động và hãm thoả mãn điều kiện sau: 


Hình 5.9. Quỹ đạo dạng 2-1-2. 


thủ, -4(, =4u) _ 


(5-10) 
qị 


4lq. — 
Dođó:  ä, „-h. _ị (5-11) 
Mặt khác, gia tốc chuyển động của khớp phải nhỏ hơn gia tốc cho phép từ 
độ bền cơ khí nên gia tốc chuyển động khớp được chọn thoả mãn bất đẳng thức: 


4lq. — 
Tan E  nê Si thước (5-12) 


3 
Từ (5-12), xác định được gia tốc khớp, theo (5-9) xác định được t, và 
theo (5-7) xác định được qạ. Với các thông số đã xác định, phương trình biểu 
diễn quỹ đạo chuyển động của các đoạn được xác định theo các phương trình 
Sau: 
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q,+ 8p? (0<t<t,) 


2 
l° t 
qŒ)= q,+~ẩiiŒ—) tt 3/2 2t) (5-13) 
1. 
q;=ữiŒ, =ÐỶ (t¿—ti<t<t,) 


5.3.4. Quỹ đạo dạng 4-1-4 


Dạng quỹ đạo 2-1-2 đơn giản và dễ thực hiện, tuy nhiên tốc độ khớp ở thời 
điểm đầu và cuối của giai đoạn khởi động và hãm thay đổi không êm, sẽ ảnh 
hưởng đến chất lượng di chuyển của robot. Dạng quỹ đạo 4-1-4 sẽ tạo ra sự thay 
đổi tốc độ êm hơn, trong đó quỹ đạo ở giai đoạn khởi động và hãm có dạng đa 
thức bậc 4. Quỹ đạo chuyển động được xây dựng như hình 5.10 phải thoả mãn 
các điều kiện đầu và cuối về vị trí, tốc độ và gia tốc của khớp: 


q(0) =qọ q(t;)=q, 
4(0) =đo đq(t,)=đ. (5-14) 
q(0) =ä; q(t.)=q. 





_*® 
ị ' — + =r CN. 
ị | t 


Hình 5.10. Quỹ đạo dạng 4-1-4. 
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Đế đơn gián cho việc tính toán, có thê chọn tham số tạ băng 1/2 khoảng 
thời gian gia tốc hoặc giảm tốc và tốc độ, gia tốc ban đầu và cuối bằng không. 
Ta cũng xác định các điểm phụ của quỹ đạo chuyển động qụ, và qụ; tại t = tụ và t 
=t,- tạ là: 

Qọi = đọ Và dạ; = q. : G-15) 

Nối qụ, và q„„ bằng một đường thẳng và xác định điểm qụ;, q,, tại t = 2tụ và 
t=t- 20. 

Bằng cách chọn điểm phụ như trên, quỹ đạo đoạn cả là một đường thẳng 
với tốc độ không đổi, quỹ đạo đoạn ac và đf có thể chọn là đa thức bậc bốn có 
dạng: 

q(Ð = a + a¡t + a; + aatŸ + a,fẾ (5-16 ) 

* Xác định giá trị qụ và q.¡: 

Như cách vẽ đã trình bày ở trên, đường nối be là đường thẳng. Giá trị qu;và 
q¡ tại t = 2tạ và t = tạ -2tạ, có thể được xác định từ giá trị đầu qụ và giá trị cuối q, 
dựa trên phương trình bậc nhất của đường thẳng be: 








đục =Qp +Tc x tạ =Qạ + to (5-17) 
~dp 
q‹ị¡ =q. — —2t tạ =q, -Atg 5-18) 
tọ 
trong đó: A= .. : (5-19) 
c “0 


- Hệ số góc của đường cd là tốc độ chuyển động khớp trong đoạn cd. 
* Phương trình đoạn cd; 
Quỹ đạo đoạn cd là đường thẳng biểu diễn bởi phương trình: 
qạ =A(t—2fs}+qụ; (5-20) 
với (2tạ<t<t,- 2ty} 
Thay (5- 19) vào và viết gọn phương trình (5 - 20) ở dạng: 
đại E Ai Ê Aj¿¡ CC ¬ 2g ) (5-21) 
với (2t <t<t,~ 2) 
Các hệ số ag.u, a„„ của phương trình đoạn cd được xác định như sau: 
pc =qạ + Ats (5-22a) 
Ai =A (5-22b) 
* Phương trình đoan ac: 
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Quỹ đạo đoạn ac biếu diễn bởi phương trình dạng đa thức bậc bốn (5 -16): 
Qu “8u, + Ai, [+ A2, [° +, PP + a,„EÊ 5-23) 
Các hệ số của phương trình đoạn ac là Âoe 8„s82; 82a. 4„„ được xác định 
từ điều kiện đầu và cuối: 


tạit= 0: 
ac (0) =qọ 
qạc(0)=0 (5-24) 
đạc (0)=0 

tại t= 2t: 


Qạc(2tg)=q,a¿(2tạ) 
đạc (to)= sa (@tg) 
Lấy đạo hàm cấp I và 2 của phương trình (5-23) và sử dụng (5-24), (5-25), 
các hệ số được xác định như sau: 


@-25) 


đoạc ` Qụ 

Äịac =0 

Âz¿„ = 0 (5-26) 

a = ——A 5-27a 
ảcd 4t ( ) 

l 

84 =————AÁ 5-27b 

4cd l6t) ( ) 


* Phượng trình đoạn dĩ: 
Quỹ đạo đoạn df biểu diễn bởi phương trình bậc bốn như quỹ đạo đoạn ac: 
Quy = Bọạ † Aar T £ Asuyt” + 8/LỄ + at 5-28) 
Với 4= t— (tc— 2to) 
Các điều kiện đầu và cuối dùng cho tín t‹ ˆa các hệ số là: 
đạt Œ = Ö) = qc (ty — 2to) 
đạt Œ = 0) = đạc (te — 2to) 


đáy ( = 0) = đạc (tc — 21a, = 0 @-29) 
qạr (2to) = qc 
qạ (2to) = 0 


Sử dụng điểu kiện đầu và cuối (5-29), tương tự như phương pháp tính 
phương trình đoạn ac, các hệ số của phương trình (5-28) được xác định như sau: 
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Ñoa£ ~ đạt 


8u = Á 
8auy = Ô (5-30) 
B8. =— ME Ê 322 sọ 

3ár 


122 
3q, —2agar — 4âurÍo 


âqdry S— 
nn: 16tỆ 


5.3.5. Quỹ đạo chuyển động qua một số điểm trung gian 


Trong mục này sẽ xây dựng quỹ đạo chuyển động của khớp từ vị trí ban 
đầu qạ qua các điểm q,, q; và dừng ở vị trí cuối cùng q, với hai trường hợp: quỹ 
đạo không qua chính xác các điểm trung gian và quỹ đạo qua chính xác các 
điểm. 

a. Trường hợp thứ nhất: xét một khớp robot không cần thiết phải di chuyển 
chính xác qua các điểm trung gian q;, q;. Tương tự như phương pháp xây dựng 
quỹ đạo 4-1-4, lựa chọn các khoảng thời gian tạ và các vị trÍ đụ, đọ; địi› địa đại: 
đạ;, q.;,q.„ như hình 5.1 1. Các giá trị qạ, và q.¿ được xác định như sau: 

đụ; = q, Và Q,¿ = q. 

Từ đó xây dựng quỹ đạo giữa các vị trí (qo, qụ;), (q¡¡, Qua); (Qại, 2) Và (q.¡, 
q,) có dạng đa thức bậc 4; quỹ đạo giữa các vị trí (qu;, qi¡), (ia› đại) Và (Qs;, đại) 
có đạng đa thức bậc 1. Các hệ số của các phương trình tương ứng với các đoạn 
được xác định theo phương pháp tương tự như trường hợp quỹ đạo 4-I -4. 





Hình 5.11. Quỹ đạo chuyển động lân cận qua các điểm trung gian. 
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b. Trường hợp thứ hai: Khớp robot cần di chuyển chính xác qua các điểm 
q, và q;. Số điểm trung gian được chọn tăng lên như minh hoa trên hình 5.12a. 
Phương pháp xác định vị trí trung gian q„ (¡ = 1, 2) được minh hoạ trên hình 
5.12b: q„ là điểm giữa của đoạn AB: điểm A là điểm cất của đường t = †, - tạ và 
đường thẳng nối 2 vị trí q,, và q, và điểm B là điểm cất của đường t = t, - tạ và 
đường thẳng nối 2 vị trí q, và q,„, (q,; khi¡ = 1). Điểm q„; xác định theo phương 
__ pháp tương tự. Tương tự như trường hợp thứ nhất, quỹ đạo giữa các vị trí (qo, 
đạ;), (dịu, đụ), (đy; Qia)›..., (q,y, q,) có đạng đa thức bậc 4; quỹ đạo giữa các vị trí 
(Quạ; Q¡¡); (Giá đại) Và (q4, q.;) có dạng đường thẳng. 





Hinh 5.12. Quỹ đạo chuyển động chính xác qua các điểm trung gian. 


5.4. Thiết kế quỹ đạo cho tay robot trong hệ toa độ Đecac 
5.4.1. Khái niệm 


Trong mục 5-3 đã trình bày phương pháp xây dựng quỹ đạo chuyển động 
cho khớp robot. Mặc dù toạ độ khớp được sử dụng trực tiếp để điều khiển tay 
robot chuyển động trong không gian. Tuy nhiên nó không thích hợp biểu diễn 
tay robot khi nó thực hiện nhiệm vụ trong môi trường làm việc vì không cho 
phép tách rời hướng và vị trí tay robot. Trong những hệ thống robot phức tạp, 
ngôn ngữ lập trình được sử dụng cho điều khiển robot thực hiện các nhiệm vụ 
(task) trong dây chuyên sản xuất. Trong các trường hợp như vậy, quá trình làm 
việc của tay robot là đi chuyển giữa các điểm đã được đặt trước trong môi 
trường làm việc với đường di chuyển giữa các điểm thông thường là các đường 
thẳng [12]. Vị trí, tốc độ và gia tốc của tay khi di chuyển giữa các điểm dọc 
theo quỹ đạo được thiết kế cho điều khiển các khớp robot. Một chu kỳ điều 
khiến có thể mô tả như sau: 
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(1) Tăng thời gian bằng t = t + AI. 

(2) Tính vị trí và hướng của tay dựa trên dạng quỹ đạo chuyển động của 
tay robot. 

(3) Tính vị trí tương ứng của các khớp bằng sử dụng phương trình động 
học ngược. 

(4) Gửi thông tin tới các bộ điều khiển vị trí các khớp robot. 

(5) Quay lại bước đầu tiên. 


Một cách tổng quát, để tay robot thực hiện chuyển động giữa điểm ban đầu 
tới điểm cuối cùng theo một đường thẳng, cần phải xác định phép biến đổi từ vị 
trí đầu tới vị trí cuối cùng và chia thành các đoạn nhỏ. Phép biến đổi tổng quát 
R giữa cấu hình của tay ở vị trí ban đầu (Tạ) đến vị trí cuối cùng (T,) là: 

T, = T,R hoặc R = TT, : (5-31) 

Ba phương pháp khác nhau được sử dụng để biến đổi phép biến đổi tổng 
thành các đoạn nhỏ [13]: 

ø Chia đường chuyển động giữa hai điểm thành một số lớn đoạn tương ứng 
với số các chuyển động vi sai. Sử dụng các phương trình dịch chuyển vi sai đã 
đưa ra ở chương 3 xác đính được vị trí và hướng tay trong mỗi đoạn nhỏ thông 
qua ma trận Jacobien: 

Dị; = Dạ hoặc Dạ = Ƒ' Dự 
dT=AT @G-32) 
Tmá = Tạụ + đT 

e Phép biến đổi R có thể được thực hiện bằng một phép biến đổi tịnh tiến 
và hai phép biến đổi quay. Phép tịnh tiến thực hiện dịch chuyển khung toạ độ 
tay từ vị trí ban đầu đến vị trí cuối cùng. Phép quay thứ nhất nhằm đặt khung 
toạ độ tay ở vị trí mong muốn và phép quay thứ hai xung quanh trục khung toạ 
tay để đạt được hướng cuối cùng của tay. 

s Phép biến đổi R được thực hiện bằng hai phép biến đổi: Phé; :jnh tiến để 
đưa khung toạ độ tay đến vị trí cuối cùng và phép biến đổi quay xung quanh 
vectơ k nhằm đạt được hướng mong muốn của tay. 

Trong mục này sẽ trình bày phương pháp xây dựng quỹ đạo chuyển động 
của robot khi thực hiện nhiệm vụ lắp ráp cần phải di chuyển qua một số điểm. 
Trước hết xác định phép biến đổi biểu diễn tay robot ở vị trí đầu và cuối, sau đó 
sử dụng phương pháp thứ hai để xây dựng quỹ đạo chuyển động. 
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5.4.2. Phép biến đối biếu diễn câu hình tay robot 


Xét một robot di chuyển để gắp các chốt đặt trên giá và đặt vào một chỉ tiết 
như hình 5.13. Điểm kẹp chỉ tiết của tay robot (điểm đích) có thể mô tả bằng 
phương trình đạng tổng quát sau: 


9T. „5T; = 0C x(D ĐTP, (5-33) 
trong đó: °T¿ - ma trận (4x4) biểu diễn vị trí và hướng của tay so với hệ toạ độ 
gốc; 


#I; - ma trận (4x4) biểu diễn vị trí và hướng của điểm kẹp so với 
khung toa độ tay robot; 

“Cpy - ma trận (4x4) biểu điễn khung tọa độ làm việc của đối tượng 
trong hệ tọa độ gốc; 

®P, - ma trận (4x4) biểu diễn vị trí điểm kẹp chỉ tiết của bàn tay robot 
trong khung toạ độ làm việc của đối tượng. 





—— 





N Phn => ———-ˆ 


Hình 5.13. Robot trong không gian làm việc. 


Trong biểu thức (5-33), nếu phép biến đổi “Tẹ kết hợp trong phép biến đổi 
®T, thì sẽ không có chứa phép biến đổi “T;. Vế trái biểu diễn vị trí và hướng của 
điểm kẹp tay robot, vế phải biểu diễn vị trí và hướng của tay robot sẽ gắp vật. 

Các phương trình được viết cho các điểm 1,2,3,4,5 có dạng: 


Tại Pạ: RB.Tạs.E = INTT.Pạ (5-34a) 
P,: RB.T,.E = BO.P, (5-34b) 
P;: RB.T¿.E = BO.P; (5-34c) 
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Ð;: RB.T,.E = BO.P, (5-34d) 
P: RB.T,.E = BR.P, (5-34e) 
P;: RB.Tạ.E = BR.P; (5S-34Ð 
trong đó: RB, INIT, BO, BR - biểu diễn khung toạ độ robot, ban đầu, ngăn chứa 

chốt và chỉ tiết được lắp so với khung toạ độ gốc; 

Tạ - biểu diễn tay robot so với khung toạ độ robot (RE); 

E - biểu diễn điểm kẹp so với khung toạ độ tay robot (T,); 

P¿ - biểu diễn điểm Pạ so với khung toạ độ ban đầu của robot (BO); 

Pụ, P;, P; - biểu diễn điểm P,, P;, P„ so với khung toa độ BO; 

P¿, P; - biểu điễn điểm P„, P; so với khung toạ độ BR. 

Xét trường hợp đơn giản: robot đi chuyển tay từ vị trí Pạ đến vị trí P,. Biểu 
diễn vị trí hiện tại (Pa) so với vị trí tiếp theo (P,) bằng các phép biến đổi P„, và 
Đại: 

Pạo: biểu diễn vị trí Pạ trong khung toạ độ INTT (chính là điểm P,) 
Pạ,: biểu diễn vị trí Pạ trong khung toa độ tiếp theo (BO) 

Như vậy phương trình (5-34a) được 

viết lại như sau: 


RB.T¿.E = INTT.Pu (5-35) 
Nếu biểu điễn Pạ so với BO: 
RB.T,.E = BO.P,, (5-36) 


Từ (5-35) nhận được: 
T¿ = RB!INTT.PEu1 5-37a) 
Từ (5-36) nhận được: 
T„ = RB'BO.P,,E,' (5-37b) 
Trong trường hợp tổng quát, Ey! có 
thể khác E,". Từ đó rút ra: 





RB'INNIT.P,E,y' = RB!BO.P,E,' Hình 5.44. Mô tả quan hệ các KTĐ. 
(5-38) 
(5-38) cho phép xác định Pạy từ P„; (Pạ): 
Pạ; = BO'INIT.P„E„'E, (5-39) 


Khi điểm kẹp chuyển động đến điểm P,, ta có phương trình mô tả vị trí 
điểm kẹp sau: 

RB.T,.E = BO.P,, tại P, (5-40) 

Để đơn giản, đạt RB.T, = °T, là phép biến đổi biểu diễn khung toạ độ tay 

robot so với hệ toạ độ gốc. Như vậy, có thể biểu diễn điểm Pạ so với khung toạ 
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độ BO thông qua phương trình: 


[T;],.E = BO.Pụ 41a) 
Phép biến đổi biểu diễn cấu hình tay robot ở vị trí Pạ là: 

[°T,]; = BO.P„.E,' (5-41b) 
Biểu điễn điểm P so với khung toạ độ BO bằng phương trình: 

[°T,],.E = BO.P,, (5-42a) 
Phép biến đổi biểu điễn cấu hình tay robot ở vị trí P, là: 

[°T,], = BO.P,.E,' (5-42b) 


Từ (5-41) và (5-42) thấy rằng, để tay robot di chuyển từ điểm Pạ đến P,, 
phép biến đổi “Tạ phải biến đổi [®T;]„ đến [°T,];, trong đó Pạ, được xác định theo 
phương trình (5-39) và trong khoảng di chuyển đó phép biến đổi E không thay 
đổi. 

Một cách tổng quát, khi cần di chuyển tay robot từ vị trí ¡ đến vị trí ¡+1, sử 
dụng C.., thay cho BO, BR,.... Phép biến đổi °T; sẽ phải thay đổi từ [°T,], đến 
[T,]„¡: 

[T;], = Cụ¡: P,;.,. TE:”,„, G-43a) 
[T:].. = Cụ. Pua- TEÌ¿; (5-43b) 
trong đó: P,¡.¡ - biểu diễn điểm ¡ so với khung toạ độ chứa điểm i+1; 
P,.i¡„¡ - biểu diễn điểm ¡+1 so với khung toạ độ chứa điểm i+1. 

5.4.3. Thiết kế đường di chuyển giữa hai điểm 

Phương pháp điều khiển tay robot di chuyển từ vị trí ban đầu, được mô tả 
bởi [?T], đến vị trí [T,].,, được thực hiện bởi 3 phép biến đổi [12], [4]: 

+ Phép tịnh tiến thực hiện dịch chuyển khung toạ độ tay từ vị trí ban đầu 

®T,}, đến vị trí cuối cùng [“T,],,¡. 

+ Phép quay thứ nhất nhằm đặt khung toa độ tay theo trục định hướng tới 
(vectơ đơn vị a) của P;„, 

+ Phép quay thứ hai để đặt tay robot theo trục định hướng (vectơ đơn vị o) 
của P..¡. 

Phép quay thứ hai tương ứng với khớp quay cuối cùng. Trong trường hợp 
khi robot nhặt chỉ tiết trên băng tải thì hướng vectơ a cố định, hướng vectơ 6 
thay đối theo chu kỳ. 

Để giải quyết bài toán di chuyển tay robot từ vị trí ban đâu [T,J, đến vị trí 
cuối cùng [”T;];,¡, dùng hàm biến đổi chuyển động DẠ), với ^. = t/T, T là thời 
gian cần thiết để di chuyển từ vị trí ¡ đến vị trí i+1; t là thời gian di chuyển thực 
tế (tại ¡, t = Ó; tại ¡ +l,t =7). 
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Sử dụng hàm chuyển động, phương trình (5-43a) được viết ở dạng: 
T,), = €.¡@). P.¡.DÓ@) TE'..¡ (5-44) 
Kết hợp (5-44) và (5-43a) dễ đàng thấy rằng tại t = 0; D(0) = I. Tại t = T, 
phương trình (5-44) có dạng: 


T:],.¡ = [TT], @)=Œ„;0). P..D@) TE`1 @G-45) 
So sánh (5-45) và (5-43b),xác định được D(1): 
D(1) = Pu, LG) (5-46) 
Đặt ma trận biểu diễn điểm P,;„, ở đạng: 
nạ O0, sa. P 
PO =A=|L PA ĐA BA Pa 5-47 
11341 0 0 0 Ị | ( } 
và ma trận biểu điễn điểm P;..,„; ở dạng: 
D017 vu cà (5-48) 
l 0 0 0 1 


Sử dụng (5-47) và (5-48) biểu thức (5-46) được viết ở dạng: 
nạn TẠOp II DI (Pn —PA) 
Dổ)~| th S5 Di Ta0h PA) 
đAng, AAOp đAan A(Dg PA} 
0 0 0 1 


(5-49) 


Hàm DA) bao gồm một phép biến đổi tịnh tiến và 2 phép biến đổi quay. Cả 
ba phép biến đổi đều là hàm của ^. Nếu ^ tỷ lệ với t, các phép tịnh tiến chuyển 
động với tốc độ dài không đối và các phép quay xe quay với tốc độ góc không 
đổi: 

+ Phép tịnh tiến biểu diễn bằng ma trận L(^. ): từ điểm P, đến P,..; 

+ Phép quay thứ nhất thực hiện sao cho vectơ tới tại P; trùng với vectơ tới 
tại P;,; bằng ma trận R,(^); 

+ Phép quay thứ hai đảm bảo vectơ định hướng P, trùng với vectơ định 
hướng P;,¡ bằng ma trận R,(A). 


Dođó:  D@)=L(Q).R,0).R¿(A) (5-50) 
Phép biến đổi tịnh tiến từ điểm P, đến P,„¡ có dạng: 
L0 0 A, 
L)= S6 là (5-51) 
6:0! 5, 
0 0 0 1 
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với À„„ À„, À„ - các thành phần tịnh tiến. 

Phép quay thứ nhất R,(^) là phép quay vectơ tới P,,„; xung quanh vectơ k 
một góc 9, trong đó k nhận được bằng quay trục định hướng (o,) quanh trục tới 
(a,) một góc w/ như minh hoa trên hình 5.15. 





—sW ky 
ke cự = ký 
0 | |k, 
l l 
Sử dụng ma trận biểu diễn phép Ky (2 
- 5 
quay xung quanh vectơ k một góc 9, x(Pa) dc 
ma trận R,() có dạng: Hinh 5.15. Phép quay của vectơ k. 
SyV(A9)+C(@0)  -SựCựVA0)  CựS@A0) 0 
“ 2 
R.@)= SụCụVA9)  C?ụV(@6)+C@0) SựụS@A0) 0 (5-52) 
—CuS(A9) —Su§(^9) CA) 0 
0 0 0 l 


Với V(A9) = vers(AÐ) = I- cos (A9) 
Phép quay Ro(^) xung quanh trục vectơ tới của P,.¡ một góc ® có dạng: 
CAO) CÁO) 0 0 


CAO À0) 0 0 
R„(1)= h ` mãi (5-53) 
0 0 0 1 


Đặt biểu thức (5-51), (5-52) và (5-53) vào biểu thức (5-50) nhận được biểu 
thức sau: 


5-54 
0 0 0 1 p 


Bà¡< b dỡ đã T 
trong đó: 
-§(A4)| S°ựV(A®) + Ca) ] +C(49)[SyCuV(A9)] 
đồ =| ~S(28)[-SuCwV(A9)]+ C(.0)| C®yV(A6) + Cá | (5-54b) 


~§(A$)[—CựS(A9)] + C(4)[SuS(A6)] 
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CựS(A9) 
đa =| SuS(A9) 


(5-54c) 
C@®) 
Â„ 
dp =| ^ (5-54đ) 
À„ 
và đñ =döxda (5-54e) 


Thay À = 1 và cân bằng D(À) từ biểu thức (5-54a) với (5-49), xác định 
được: 


x =n,(Pp —PA) 
y=OA(Pg —PA) (5-55) 
z=8A(Pa — PA) 


1/2 
0= aua2([(i #ạ)” +(6Ạ.8a} |, 3) (0<ð<®) 


\ =atan 2(G¿ .äp,1 .âg } (—x<St<®) (5-56) 
©=atan2(S¿,C¿) (—Rm<¿<) 
M.UE 


Sở =—SựCuV(@)(,..ñg) + C°yV(6) + C6 |(6..ñạ)~ SỤS9(ã, -Rp) 


Cộ=~SCựV(6Xñ,.6ạ)+[C®V(6) + C9 |(öa.ðạ)~SS6(ã, .8ạ) 
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Bài tập 


5.1.Thiết kế quỹ đạo chuyển động dạng đa thức bậc 3 cho khớp 1 của robot 6 
bậc tự do từ góc ban đầu 50° đến góc cuối cùng 802 trong thời gian 3 s. Xác 
định góc quay, tốc độ và gia tốc góc của khớp ở thời điểm 1, 2, 3 s. Biết 
rằng robot chuyển động từ trạng thái đứng yên và đừng ở vị trí cuối cùng. 

5.2. Khớp thứ hai của robot 6 khớp quay từ góc ban đầu 20° đến góc cuối cùng 
§0° trong thời gian 5 s, sau đó quay tiếp đến góc 25” trong 5 s. Xây dựng 
quỹ đạo chuyển động dạng đa thức bậc 3 cho khớp 2 và vẽ đường biểu diễn 
vị trí, tốc độ và gia tốc. 

5.3. Đa thức bậc 5 được sử dụng để điều khiển chuyển động của khớp robot 
trong không gian khớp. Xác định các hệ số của đa thức bậc 5 để khớp quay 
từ góc ban đầu 0° đến góc quay cuối cùng 75" trong 3 s. Biết rằng tốc độ 
ban đầu và cuối cùng bằng không và gia tốc ban đầu và cuối cùng là 10 
m/$. 

5.4. Xây dựng quỹ đạo dạng 2-1-2 của khớp I robot quay từ góc ban đầu 40° 
đến góc cuối cùng 120” trong 4 s với tốc độ lớn nhất là 10/s. Vẽ đường 
biểu điễn vị trí, tốc độ và gia tốc chuyển động của khớp. 
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CHƯƠNG 6 
ĐIỀU KHIỂN CHUYỂN ĐỘNG ROBOT 


6.1. Bài toán điều khiển chuyển động 


Chức năng của hệ thống điều khiển chuyển động là đảm bảo tay robot (End 
effector) chuyển động bám theo quỹ đạo đặt trước trong môi trường làm việc 
(không gian làm việc). Chuyển động của tay robot được thực hiện nhờ các hệ 
thống truyền động khớp robot. Trên cơ sở đó, có hai dạng hệ thống điều khiển 
chuyển động: hệ thống điều khiển ở không gian khớp (hình 6.1) và hệ thống 
điều khiển ở không gian làm việc (hình 6.2). 

Ở hệ thống điều khiển khớp, đại lượng điều khiển là vị trí của khớp robot: 
góc quay đối với khớp quay; độ dịch chuyển thẳng đối với khớp tịnh tiến. Bộ 
điều khiển được thiết kế đảm bảo vị trí khớp luôn bám theo vị trí đặt, tức là sai 
lệch vị trí khớp hội tụ về không với thời gian nhỏ nhất. Vị trí đặt của khớp được 
tính toán từ lượng đặt vị trí của tay robot trong không gian làm việc thông qua 
khâu tính toán động học ngược. Ưu điểm của phương pháp điều khiển ở không 
gian khớp là bộ điều khiển tác động trực tiếp đến hệ thống truyền động của 
khớp. Tuy nhiên, hệ thống điều khiển này khó đảm bảo độ chính xác vị trí của 
tay khi tồn tại các sai lệch trong cơ cấu cơ khí (khe hở trong hộp số,...), hoặc 
thiếu thông tin về quan hệ vị trí giữa tay robot và đối tượng. 

S — u § 





Hình 6.1. Sơ đồ khối hệ thống điều khiển ở không gian khớp. 


Hệ thống điều khiển không gian làm việc có chức năng duy trì trực tiếp sai 
lệch vị trí của tay robot trong không gian làm việc bằng không. Lượng đặt của 
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hệ thống điều khiển là vị trí đặt tay trong không gian làm việc và lượng phản 
hồi là vị trí thực của tay. Khâu tính toán động học ngược sẽ thuộc mạch vòng 
điều khiển phản hồi. Ưu điểm của phương pháp điều khiến này là tác động trực 
tiếp các biến không gian làm việc. Nhược điểm là khối lượng tính toán lớn do 
tồn tại khâu tính toán động học ngược trong mạch vòng điều khiển. 








Bộ điều 
khiển 


„ Ặ ẶẪ{ƒ{ƒÝẳnng 


Hình 6.2. Sơ đồ khối hệ thống điều khiển ở không gian làm việc. 


Chương 4 đã dẫn ra các mô hình toán học mô tả cơ cấu chuyển động và cơ 
cấu truyền động của robot: Mô hình toán học dạng tổng quát có tính đến sự 
ràng buộc và phi tuyến của cơ cấu chuyển động ((4-73), (4-98)) và mô hình 
_ toán học độc lập khi tỉ số truyền của bộ truyền lực lớn (4-101) hoặc (hình 4-5), 
trong đó thành phần mômen (4-102) gây ra bởi sự ràng buộc giữa các khớp 
được coi là các nhiễu đối với hệ thống điều khiến và có thể bổ qua. Trên cơ sở 
các mô hình động lực học đó, hai hệ thống điều khiển chuyển động được xây 
dựng [6]: Hệ thống điều khiển "phân tán” và "tập trung". Hệ thống điều khiển 
phân tán được xây dựng cho robot khi coi robot gồm các khớp độc lập, tức là bỏ 
qua sự ràng buộc giữa các khớp. Đối với các robot có tỉ số truyền của bộ truyền 
nhỏ, robot là một hệ thống có tính ràng buộc và phi tuyến cao, hệ thống điều 
khiển tập trung sẽ được xây dựng. 


6.2. Hệ thống điều khiển độc lập các khớp 
6.2.1. Mô hình động cơ truyền động khớp 


Đối với các robot có tỉ số truyền của bộ truyền lớn, có thể coi hệ thống 
robot n bậc tự do sẽ gồm n hệ thống độc lập và là hệ thống 1 đầu vào/1 đầu ra 
(SISO) và sự ràng buộc giữa các khớp được coi là thành phần nhiễu. Trong 
trường hợp đơn giản, nếu bỏ qua thành phần mômen nhiễu, sơ đồ cấu trúc mô tả 
một khớp robot có dạng như hình 6.3. Sơ đồ hình 6.3 mô tả một khớp bất kỳ và 
không dùng ký hiệu chỉ số ¡ trong các đại lượng của khớp. 


l5 





Hình 6.3. Sơ đồ cấu trúc của hệ thống chuyển động của một khớp. 


Hàm truyền mô tả động cơ truyền động khớp có đạng: 
We(@)= ®o(p (p) _ Kp 
Up(p) p(I+Tạp) 
RJ 
KK 


(6-1) 


trong đó: Kẹ ¬ Tp= 


e 


; J=Jp.+k}H, 


Hạ - thành phần (¡, ¡) tương ứng với khớp ¡ của ma trận H chỉ chứa các 
hằng số khớp ¡; 
Ôp- góc quay của động cơ truyền động khớp. 
6.2.2. Hệ thống điều khiển phản hồi 
Sơ đồ hệ thống điều khiển phản hồi cho khớp có dạng điển hình như hình 
6.4. Hệ thống điêu khiển gồm 3 mạch vòng điều chỉnh gia tốc, tốc độ và vị trí 
khớp với 3 bộ điều khiển tương ứng là R,(p), R„(p) và R ríP). Cấu trúc bộ điều 
khiển mạch vòng trong cùng R,(p) có đạng tỷ lệ - tích phân (PD để nhận được 
sai lệch tĩnh bằng không. Các bộ điều khiển vòng ngoài có thể có cấu trúc tỷ lệ (P). 
__ Các hệ số K, K¿ và K, tương ứng là hệ số phản hồi Bia tốc, tốc độ và vị trí khớp. 
Cấu trúc các bộ điều khiển có dạng: 


1+Tp,p 


Rg()=Km,  Ru(p)=K,, R,=Kg (6-2) 


â 


Sơ đồ cấu trúc ở hình 6.4 được biến đổi thành sơ đồ cấu trúc ở hình 6.5. 
Từ sơ đồ cấu trúc hệ thống hình 6.5, hàm truyền đối tượng điều khiển của 
hệ thống được xác định ở đạng: 





6 K 
pr(P)=——— bÉP) _„ ° (6-3) 
Up(Ð) - Ty 
Lư gu Tpp 
+KpKg¿K,)|1+ =_ 
đ+KpKgK,) 
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Hình 6.5. Sơ đồ hệ thống điều khiển phản hồi. 


Hàm truyền mạch thắng có dạng: 


KggKguKs; + Tp,p) 
2 


Wr(p)= Wur(p) (6-4) 


Hàm truyền mạch phản hồi có dạng: 
K 
Wr() =K, +-—9È (6-5) 
K§p 
Bằng chọn Tạ, = Tọ sẽ khử được hằng số thời gian lớn của hệ thống. Hàm 
truyền kín của hệ thống được viết ở dạng sau: 
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Ôp(p) _ Wr(p) _ l/K; 
6p(p) 1+ Wr(p)Wr(p) 1 ". Ấn ( T KpKp¿K,) 2 
KgpK, KpKnpK,KạuKqp 
Hàm truyền theo tín hiệu nhiễu tải có dạng: 
Rp 
9s) _ KnKzpKpKr„K,(+Tạ,p) 
Md(@ ¡„ K. ,„ (+KoKK) ¿ 
KqgK› KpKngK;KpuKu 

Hàm truyền hệ kín (6-6) có đạng khâu bậc 2 chuẩn: 

1/K, 
i+ 25E + K. 

®, (0 


WM.(0)= 








(6-6) 





W(p)= (6-7) 


Wy (P) = (6-8) 


So sánh (6-7) và (6-8) nhận được các biểu thức sau: 
K (3 
KhRpK» 





1+KpKpaKạ (6-2) 


+ 
đìn 
Ko 
KpKnqpKpKnuKga œ2 


Các biểu thức (6-10) cho phép xác định tham số các bộ điều chỉnh gia tốc, 
tốc độ và vị trí theo các chỉ tiêu yêu cầu (độ quá điều chỉnh ơ và thời gian quá 
độ Tạ„) khi đã chọn các hệ số phản hồi K, K„, K,. 

Ví đụ: Để đảm bảo đạt được độ quá điều chỉnh ø < 20%, hệ số Suy giảm sẽ 
được chọn nằm trong phạm vi: š = 0,5 - 0,7. Tẩn số dao động được tính theo 
thời gian quá độ yêu cầu và hệ số suy giảm š: 

dạ, = : 
Tụ 





(6-10) 


Từ hệ số suy giảm và tần số dao động sẽ xác định được các hệ số Kạ„, Ko, Kạu, 

Phương pháp thiết kế hệ thống điều khiển xây dựng với mô hình hệ thống 
lý tưởng, tức là bỏ qua hằng số thời gian điện từ động cơ, hằng số thời gian bộ 
biến đổi, tính đàn hồi cùa bộ truyền lực. Điều đó có nghĩa là để đáp ứng các yêu 
cầu chất lượng đặt ra, hệ thống cẩn có hệ số phản hồi lớn. Tuy nhiên sẽ không 
thích hợp trong thực tế vì chất lượng hệ bị suy giảm gây bởi đặc tính động học 
của hệ thống cơ điện, ví dụ tồn tại khâu đàn hồi trong bộ truyền cơ. 
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6.2.3. Hệ thống điền khiển tiền định (Feedforward) 

Hệ thống điều khiển phản hồi có thể đảm bảo độ chính xác về quỹ đạo 
chuyển động. Tuy nhiên để đạt độ chính xác di chuyển cao với gia tốc và tốc độ 
khớp cao, hệ thống điều khiển phản hồi (hình 6.4) không thoả mãn. Hệ thống 
điều khiển bù tiền định sẽ cho phép giảm sai số vị trí. 

Hàm truyền hệ thống kín với các phản hồi gia tốc, tốc độ và vị trí (mục 
6.2.2) có dạng: 


vùi GHI) 2= th (6-11) 
" ...... 1N 
Kq,;K; KpKmpK,KguẾp; 


Để đảm bảo khớp robot di chuyển theo quỹ đạo đặt trước (Ôp(Ð = 0;`(Ð), 

thích hợp nhất xác định tín hiệu đặt theo biểu thức sau: 
Đp®) + mg : (I+KpKp¿K;}) p' 
9;(œ) : Kng KpKnpKgpuÉp; 

Bằng kết hợp biểu thức (6-11) và (6-12), có thể kiểm nghiệm được góc 
quay của động cơ luôn bám theo lượng đặt (9;(1) = 0p (9). 

Sử dụng (6-12), sơ đồ toàn hệ thống điều khiển có dạng như hình 6.6. 

Trong sơ đồ hình 6.6, M(p) là hàm truyền của động cơ kết hợp với bộ 
truyền và khớp robot. Sơ đồ hình 6.6 có 3 mạch vòng điều chỉnh, chỉ cần một 
tham số Ko cho xây dựng khâu bù tiền định. Độ chính xác bám quỹ đạo sẽ đạt 
được khi các tham số sử dụng trong bộ điều khiển và khâu bù tiền định luôn 
bằng tham số của đối tượng. Khi có sai lệch về tham số, độ chính xác bám quỹ 
đạo sẽ giảm. Các khâu bão hoà trong hệ thống điều khiển có tác dụng hạn chế 
các đại lượng của hệ thống trong quá trình quá độ: tốc độ, gia tốc, điện áp động 
cơ. Các khâu hạn chế này rất cần thiết cho robot công nghiệp khi robot cần di 
chuyển điểm - điểm, vì trong trường hợp này không quan tâm đến dạng quỹ đạo 
mà chủ yếu yêu cầu chuyển động nhanh ở mức cao nhất, tức là cần tạo gia tốc 
lớn nhất, hoặc dòng điện động cơ trong quá trình quá độ lớn nhất. 

Từ sơ đồ cấu trúc hình 6.6 có thể viết được phương trình sau: 
" Kạa{ + Tạạp) 

p 

Khai triển biểu thức 6-13, nhận được sơ đồ cấu trúc hình 6.7, trong đó chỉ 

sử dụng tín hiệu phản hồi vị trí. l 


(6-12) 


KKuKuz + K„Kz„pE)+ K,p2e+ : (6-13) 


Dˆ`Ra 


Up 
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K 





KpaKnpKguT Ra + 


Hình 6.7. Sơ đồ hệ thống điều khiển dạng PIDD”. 


Sơ đồ hệ thống điều khiển hình 6.7 là một dạng hệ thống điều khiển chuẩn 
trong robot công nghiệp. Hệ số khuếch đại cần phải chọn lớn nhằm nâng cao độ 
chính xác và giảm sự ảnh hưởng của sự ràng buộc các khớp đến chất lượng hệ 
thống. Giá trị lớn nhất của hệ số khuếch đại cần phải chọn phù hợp với các yếu 
tố nhiễu của cảm biến, ảnh hưởng của đáp ứng động học hệ thống... 


6.3. Hệ thống điều khiển tập trung 

6.3.1. Hệ thống điều khiển phản hồi 

Khi thiết kế hệ thống điều khiển, có thể bỏ qua động học của cơ cấu chấp 
hành: quán tính của động cơ và bộ biến đổi. Như vậy chức năng của bộ điều 
khiển là tạo ra một mômen cần thiết để truyền động khớp robot đảm baỏ khớp 
robot luôn bám theo vị trí đặt. 

Sơ đồ khối tổng quát của hệ thống điều khiển phản hồi robot được trình 
bày trên hình 6.8. q¿ là vectơ tín hiệu đặt vị trí của các khớp (qạ =ð¿ đối với 
khớp quay và q¿ = rạ đối với khớp tịnh tiến), q là vectơ vị trí thực của các khớp 
robot tương ứng là 9 và r đối với khớp quay và tịnh tiến; M là vectơ mômen của 
các khớp quay và lực đối với khớp tịnh tiến. 


Bộ điểu |__ — _ 
khiển 
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Hình 6.8. Sơ đồ khối tổng quát của hệ thống điều khiển phản hồi. 


Phương trình động lực học tổng quát của robot có dạng (4-73): 
M=H()8 + V(,ä) +G(4) (6-14) 
Giả thiết thành phần mômen trọng lực G(p) được bù hoàn toàn, sơ đồ hệ 
thống điều khiển phản hồi với cấu trúc điều khiển PD có dạng đơn giản như 
hình 6.2 a. Trên sơ đồ đó, tín hiệu đặt vị trí q; được so sánh với vị trí thực của 
khớp q, sai lệch được đặt vào khâu khuếch đại với hệ số K;. Tín hiệu ra của 
khâu tỷ lệ được cộng đại số với tín hiệu tỷ lệ với tốc độ của khớp và đặt tới cơ 
cấu chấp hành của robot: 
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Mạ =Kp(q¿ —)- Kpä (6-15a) 
hoặc viết cho một khớp thứ ¡ có: 
Mạ, = Kạ; (qạ ~ qị)— Kế, (6-15b) 
trong đó: K; = diag(K„¡, K„a„...., K,„) - ma trận đường chéo các hệ số khuếch đại 
của từng khớp riêng biệt; 
Ks = diag(E, Kự;,...., Ku„) - ma trận đường chéo các hệ số đạo hàm 
của từng khớp riêng biệt. 

Tín hiệu đầu ra của bộ điều khiển (6-15) là mômen truyền động cho robot 
mô tả bởi mô hình động lực học trực tiếp R (6-14), đầu ra của mô hình robot R 
là vị trí thực của các khớp. 

Luật điều khiển (6-15) của một khớp không phụ thuộc vào mô hình robot, 
chỉ phụ thuộc sai lệch vị trí của khớp đó. Do đó bộ điều khiển độc lập với các 
khớp của robot, nên gọi là bộ điều khiển PD độc lập. Hệ thống với cấu trúc luật 
điều khiển (6-15) có độ tắt dần lớn sẽ không thích hợp với một số đạng robot. 
Một dạng hệ thống điều khiển khác trình bày trên hình 6.9b với bổ sung thêm 
tín hiệu đặt tốc độ và sai lệch tốc độ được đặt vào khâu khuếch đại Kạ. Khi đó 
cấu trúc bộ điều khiển có đạng tỷ lệ-đạo hàm (PD) kinh điển: 


Mạ =K;E+ Kp£ (6-16a) 
hoặc viết cho một khớp thứ i có: 
Mụy; = K j£¡ + Ku, (6-16b) 


trong đó: £ = qa - q - sai số vị trí của khớp robot. 
£=¿-—4 - sai số tốc độ khớp robot 
K;= diag(K.¡, K„z„...., K,¿) - ma trận đường chéo các hệ số khuếch đại 
của từng khớp riêng biệt; 
Kp = diag(K, Kựz;....., Kạ,) - ma trận đường chéo các hệ số đạo hàm 
của từng khớp riêng biệt. 





Hinh 6.9. Sơ đồ cấu trúc hệ thống điều khiển robot với bộ điều khiển PD: 
a- bộ điều khiển với phản hồi tốc độ; b- bộ điều khiển PD với lượng đặt tốc độ. 
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Hệ thống điểu khiển với cấu trúc bộ điều khiển (6-16) ổn định tuyệt đối 
toàn cục. Thực, vậy, chọn hàm Liapunov có dạng: 
V.= 2Œ Kye +ä"Hệ) (6-17) 
Hàm V, biểu thị tổng năng lượng của hệ thống robot: Thành phản 
2ŒTK;ẽ) tỷ lệ năng lượng đầu vào và thành phần - ('Hã) là động năng của 
robot. K, và H là các ma trận hệ số dương, nên hàm V, > 0 với q z Qạ. 


Tính đạo hàm cấp 1 hàm V,, nhận được: 
I 


, Ta... 5...5... 
V.= 2° Kpẽ +2 'Kẹẽ + z1 Hạ + 21 Hà + z3 Hạ (6-18) 
Do tính đối xứng của các thành phần S KẾ äT H, (6-18) được rút gọn 
ở dạng: 
Ý,=ÊTK,E+ sả H§+ Ä'H§ (6-19) 
Thay phương trình động lực học dạng tổng quát (6-14) vào phương trình 
(6-19), với giả thiết không có thành phân mômen trọng lực G(p), nhận được 
phương trình sau: 
Ÿ, =ÉTK,E+ sã H§: ä"[M - V(q,ä)] (6-20) 
Sử dụng thuộc tính của phương trình động lực học [6] và ấp dụng luật điều 
khiển (6-16), phương trình (6-20) được biến đổi thành đạng: 
_ —s ~ Ï— ỷ—_ m—-— 1 _— Tư—x—~ 
V¿ =qˆKp£-q”C(4.4)4 +24'H()8 
1 (6-21) 
=-4'K,ê+§'Gñ-Ol 
trong đó: V(q,q)= C(,đ). 
Do ma trận sử —C là ma trận đối xứng ngược, g'cn ~C)q=0 với mọi 
q, nên từ (6-21) nhận được: 
V,=-q'Ksq «0 (6-22) 
Bất đẳng thức (6-22) cho thấy rằng hệ thống ổn định tuyệt đối. 
Từ (6- L7) và (6-22) cho thấy rằng, mức độ dương của Vị phụ thuộc vào Kp; 
mức độ âm của Ÿ, phụ thuộc vào Kp. Do đó tăng tốc độ hội tụ bằng tăng giá trị 
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ma trận hệ số Ko. Nâng cao độ chính xác tỉnh của hệ thông điêu khiên đạt được 
bằng tăng hệ số K; của khâu khuếch đại. Tuy nhiên, Kạ và Kp lớn sẽ làm giảm 
độ ồn định và chất lượng quá trình quá độ như độ quá điều chỉnh và thời gian 
quá độ tăng. Mặt khác, sự tồn tại ảnh hưởng của thành phần mômen trọng lực 
cũng làm giảm chất lượng của hệ thống. Bộ điều khiển có cấu trúc PID sẽ khắc 
phục được nhược điểm của bộ điều khiển PD. Khi đó phương trình mômen bộ 
điều khiển có dạng: 


{ 
Mụ, =K;E + Koẽ +K, [E(txit (6-23) 


trong đó K, = diagŒ, K„,...., K,„) - ma trận đường chéo các hệ số tích phân. 

Bằng phương pháp tương tự, có thể chứng minh hệ thống với bộ điều khiển 
(6-23) ồn định. Tác dụng của khâu tích phân của (6-23) là hiệu chỉnh chất lượng 
hệ thống khi sự ảnh hưởng của mômen trọng lực không được bù hết. 


Ví dụ ó.Ï: Thiết kế bộ điều khiến phản hồi cấu trúc PD (Tỷ lệ -đạo hàm) 
cho robot hai thanh nối trong mặt phẳng có 
cấu hình như hình 6.10 với các tham số 
như sau: 

m{l = 2,5 kg; m2 = 1,5 kg; mt = 0,5 kg; 
al =0,25 m; a2= 0,15m 

+Hệ phương trình động lực học robot 
có dạng tổng quát như (4-73): 

M=H()8+V(,4)+G() (6-24) 

Sử dụng hệ phương trình động lực học 

robot hai thanh nối dạng tổng quát (4-26) 





+ (4-29), nhận được các ma trận của hệ Hình 6.10. Cấu hình robot 2 thanh nối. 
phương trình động lực robot như sau: 

lc th H; | W= = vi GĂ= lM (6-25) 

Hạ, H„ hạnÐ; E2 

Với: Hạ; =m,aỷ +(m, +m,)\Ía +a? +2a¡a; cos6, Ì (6-25a) 

H:; =H;¡; =(m, +m,)(a) +âya; cos6; (6-25b) 

Hạ; =(m; +m,)a; (6-25c) 

hịyy = hy; =—h¿¡; = -(m; +m,)a¡ã; sin Ô; (6-25đ) 


8, = m;ga¡ cos0, + (m, + m, )gÌa, cos9, +a, cos(0,+6,)] — (6-25) 
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8; =(m; +m,)ga; cos(9, + 9, } (6-25Đ 

+ Phương trình luật điều khiển có đạng (6-19): 

Mạ =K;E+Kpẽ (6-26) 

Với: e=q¿-—q - sai số vị trí của khớp robot; 

£ =¿ —Ñ - sai số tốc độ khớp robot; 
K; = diag(K,¡, K,;) - ma trận đường chéo các hệ số khuếch đại; 
Kb = diag(K¡, K,;) - ma trận đường chéo các hệ số đạo hàm. 

+ Quỹ đạo đặt cho hai khớp có dạng 2-1-2 được tính toán đảm bảo tay 
robot đi chuyển từ vị trí ban đầu (0,2 0) đến vị trí cuối cùng (0 0,1) trong thời 
gian 2 s. 

+ Ma trận hệ số khuếch đại tỷ lệ và đạo hàm được lựa chọn: 


É 200 0 `. 3 0 
= Và — 
ý 0 200 #" lờ -3 


+ Chương trình mô phỏng được viết bằng ngôn ngữ MATLAB minh họa ở 
phụ lục (P L2.....). 

Hình 6.11 là các kết quả mô phỏng góc quay các khớp 1,2; sai lệch góc 
quay 1,2 và mômen khớp 1,2. 
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Hình 6.11. Kết quả mỏ phỏng của ví dụ 6.1. 
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6.3.2. Hệ thống điền khiển mômen tính toán 

Phương pháp mômen tính toán cũng mang tên là phương pháp điều khiển 
` động lực học, phương pháp điều khiển theo mô hình {4], [6],[14]. Nguyên lý cơ 
bản của phương pháp điều khiển là lựa chọn luật điều khiển sao cho khử được 
các thành phần phi tuyến của phương trình động lực học robot và phân ly đặc 
tính động lực học các thanh nối. Kết quả là sẽ nhận được một hệ thống tuyến 
tính, dễ dàng thiết kế theo các phương pháp kinh điển của hệ thống tuyến tính 
đảm bảo độ chính xác chuyển động yêu cầu. 

Dựa trên phương trình động lực học (6-14) của robot, giả thiết tất các các 
tham số robot đã biết hoặc được xác định chính xác, phương trình mô tả bộ điều 
khiển mômen tính toán được chọn như sau: 

Mạ, =H()Ũ + V(,8)+ G(4) (6-27) 

Cân bằng mômen ở phương trình (6-27) và (6-14), H(q) là ma trận thực 
dương có thể lấy nghịch đảo ([6]), ta nhận được phương trình vi phân tuyến tính 
cấp 2 mô tả động học của hệ thống kín như sau: 

q=U (6-28) 

(6-28) là hệ phương trình vi phân tuyến tính cấp 2 độc lập đối vơí các khớp. 
Do đó có thể thiết kế các bộ điều khiến độc lập có cấu trúc PD hoặc PID cho 
từng khớp. Luật điều khiển phụ Ú có cấu trúc PID như sau: 


t 
Ũ =Ñ, + K;ẽ + Koẽ +K, [E(rxr (6-29) 
trong đó: q, - đạo hàm cấp 2 của tín hiệu đặt vị trí. 
Sơ đồ khối của hệ thống điều khiển mômen tính toán (Điều khiển động lực 


học) trình bày trên hình 6.12. Kết hợp các phương trình (6-29) và (6-28), 
phương trình ví phân sai số vị trí của hệ thống kín có dạng như sau: 


£# +Kgẽ+ K;ẽ +K, =0 (6-30) 
Phương trình đặc tính ở dạng toán tử Laplace là: 

s°I+Kps°+Kạs +K, =0 (6-31) 
Phương trình (6-3L) viết cho từng khớp như sau: 

s”+K„s + K„js + Kặ =0 (6-32) 


Các hệ số K.„ K¿, K, có thể được tính toán theo các tiêu chuẩn ổn định và 
hội tụ bằng đặt nghiệm của phương trình ở các vị trí mong muốn bên trái mặt 
phẳng phức. 


162 






.^I 


Tính 
H()U + V,j)+G() 





„ĐỊ- 


Hình 6.12. Sơ đồ cấu trúc hệ điều khiển mômen tính toán. 


Nhược điểm của phương pháp điều khiển mômen tính toán là cần phải biết 
đầy đủ và chính xác các thông số cũng như đặc tính động lực học của robot. 
Tuy nhiên các thông số động học của robot thay đổi trong quá trình làm VIỆC, 
nên để khử hoàn toàn các thành phần phi tuyến và phân ly hoàn toàn động lực 
học của các khớp theo phương trình (6-14), cần phải ước lượng chính xác các 
thông số của robot trong quá trình làm việc. Hơn nữa, thuật toán tính toán luật 
điều khiển mômen tính toán sẽ liên quan đến các phép toán trung gian, nên phải 
thực hiện các phép nhân vectơ và ma trận phụ, khối lượng tính toán sẽ lớn. Thời 
gian tính toán lớn sẽ hạn chế khả. năng áp dụng phương pháp điều khiển này 
trong các robot công nghiệp. 

Ví đụ 6.2: Thiết kế bộ điều khiển phản hồi động lực học ngược và bộ điều 
khiển tuyến tính vòng ngoài cấu trúc PD CTỷ lệ -đạo hàm) cho robot hai thanh 
nối trong mặt phẳng có cấu hình như hình 6.10 với các tham số như ví dụ 6.I: 

ml = 2,5 kg; m2 = 1,5 kg; mt = 0,5 kg: a1 = 0,25 m; a2= 0,15m 

+ Hệ phương trình động lực học robot có dạng tổng quát như (4-73): 

M= H()8 + V(,3) + G(4) (6-33) 

Các ma trận thành phần có dạng (6-25). 

+ Luật điều khiển có dạng (6-27): 


Mụ, = H()Ú + V(q,8) + G(q) (6-34) 
Tín hiệu điều khiển phụ được lựa chọn có cấu trúc PD như sau: 
Ư = K;£ + Kp;š (6-35) 


với: £=qa-q - sai số vị trí của khớp robot; 
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#=¡—4 - sai số tốc độ khớp robot; 
K; = điagŒ,¡, K,¿) - ma trận đường chéo các hệ số khuếch đại; 
Kp=diag(„¡, K„ø;) - ma trận đường chéo các hệ số đạo hàm. 

+ Quỹ đạo đặt cho hai khớp có dạng 2-1-2 được tính toán đảm bảo tay 
robot đi chuyển từ vị trí ban đầu (0,2 0) đến vị trí cuối cùng (0 0,1) trong thời 
gian 2 s. 

+ Các hệ số khuếch đại tý lệ và đạo hàm được lựa chọn theo các chỉ tiêu 
quá trình quá độ mong muốn ({8],[9]): 

kụ = 0i và k„ =2k;@„; 

Chọn hệ số suy giảm  = 0,7 và thời gian quá độ t,„ = I s, tính được các ma 

trận hệ số tỷ lệ và đạo hàm như sau: 


3265 0 8 0 
Kp= và Kp= 
0 32/65 0 8 


+ Hình 6.13 là các kết quả mô phỏng góc quay các khớp 1,2; sai lệch góc 
quay 1,2 và mômen khớp I.2. 
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Hình 6.13. Kết quả mô phỏng của ví dụ 6.2. 
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6.3.3. Phương pháp điều khiển phân Iy phi tuyến 
Phương pháp điều khiển phân ly phi tuyến được xây dựng trên cơ sở lý 
thuyết điều khiển phân ly cho hệ phi tuyến ([4]) bằng phản hồi tuyến tính tín 
hiệu ra. Phương trình động lực học của robot có dạng (6-14): 
M=H()8 + V(q,4)+G() (6-36) 
Chọn biến đầu ra là biến khớp q: y=q 
Đo ma trận H(q) luôn là ma trận không đơn nhất nên từ phương trình 
(6-36) có thể viết được phương trình gia tốc của robot: 
q=-H')| V{,8)+ G() |+ H”')M() (6-37) 
Hệ phương trình (6-37) gồm các phương trình vi phân cấp 2 cho mỗi biến 
khớp, vì vậy sau 2 lần vi phân tín hiệu đầu ra y; cho mỗi biến khớp, hệ số của tín 
hiệu vào H sẽ khác không, có nghĩa là tín hiệu vào lần đầu tiên xuất hiện trong 
phương trình đầu ra. Đạo hàm cấp 2 của từng biến đầu ra khớp ¡ được biểu diễn 
bằng phương trình sau: 
ÿ, =ä, =-[H"'()],[V,(q,đ)+G,(đ)] + [H”()]I.M;(Ð (6-38) 


=P;(X)+ H;(q)M,(t) (6-39) 

trong đó: D; (X) = -[H”'(4)],[V,(đ,đ) + G,(4)] (6-40a) 
X' =[qT() §T@)]. (6-40b) 

H;(4) =[H"'(q) | _ - hàng thứi của ma trận H”'4) (6-40c) 


Kết hợp n phương trình (6-40) cho từng khớp, có thể viết phương trình 
vectơ tổng quát cho robot ở dạng sau; 
ÿ=Ñ=-C'(X)+Hˆ')M@) (6-41) 
Tín hiệu ra M„(©) của bộ điều khiển được chọn để đảm bảo cho hệ thống 
phân ly, tức là động lực học các khớp độc lập nhau: 


Mạ (1)=-H“!(4)LC'(Ä)+aˆG9~ AWG | (6-42) 
trong đó: œ”(X) là vectơ (nx1) có thành phần thứ ¡ là: 

œ¡ CX) =0,,q¡+0ú, '(6-43a) 

A =diag(^.,À¿.....À„} (6-43b) 


W(Ð - vectơ (nx1) tín hiệu vào. 
Sử dụng (6-40), phương trình (6-42) viết lại ở dạng sau: 
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Mụ, =-H@)‡-H"'(4)[V(,4) + G4)}+ «` ỞØ)=AW(Đ} 
= V(,g)+ G(q)~ H)[œ”(X) - AW(Đ] 
Kết hợp với (6-43), phương trình (6-44) được viết cho khớp thứ ¡ như sau: 
Œ%¡q,(9 + œg,()— Äw¡@) 


(6-44) 


Mu„¿()= V.,8)+G,@)-[H().. HụQ)] 
0Œaq„€Ð) + Gadp(Ð) —À„,W,(Ð) 
(6-45) 
Phương trình (6-45) cho thấy rằng tín hiệu điều khiển M,(Ð của khớp ¡ chỉ 
phụ thuộc vào các thành phần động lực học (V,(q,q), G,()) của khớp ¡ và tín 
hiệu vào phụ W(t). Thay phương trình (6-44) vào phương trình động lực học (6- 
14) ta nhận được: 
HQ)8 + V(@,4) + G(4) = V(,ä) + G(8)- H()[ ø”(X)~AW() | (6-46) 
Từ (6-46), sau một số biến đổi, nhận được phương trình ở dạng khai triển 
Sau: 


đị + 2q) + œ8¡()~X¡w¡@) 


H(qđ) # =0 (6-47) 
đa ++aq,€) + œinqa(Ð—À„W„@) 

Ma trận H(q) không đơn nhất nên phương trình (6-47) có thể được viết tách 
riêng cho từng khớp ở đạng ở dạng sau: 

đ¡ + œ,¡g¡() + œ¡đŒ)— À¡w¡@) = Ô @=1/2,....n) (6-48) 

Phương trình (6-47) biểu điễn mối quan hệ vào-ra phân ly của hệ thống. 
Các hệ số œ,„ œ,, và À, của khớp ¡ có thể chọn thoả mãn các tiêu chuẩn ổn định 
kinh điển. Như vậy robot có thể được mô tả là một hệ thống gồm n hệ thống 
nhỏ độc lập, phân ly với luật điều khiển (6-44) hoặc (6-45). 

6.3.4. Hệ thống điều khiển bù trọng tực với bộ điều khiển PD 

Bài toán đặt ra là xác định cấu trúc bộ điều khiển đảm bảo hệ thống ổn 
định tuyệt đối xung quanh điểm cân bằng (q = q,), không phụ thuộc vào khối 
lượng của các thanh nối và tải. Luật điều khiển được xây dựng dựa trên tiêu 


ló6 


chuẩn ổn định Liapunov trực tiếp. 
Đặt biến trạng thái của hệ thống là: [£T,qT]ï 
với: E=q¿~q - vectơ sai số vị trí của khớp robot. 
Ñ - vectơ tốc độ khớp robot. 
Chọn hàm Lìapunov có dạng: 


N_ 2ử'H@á +2E'K,E >Úvới q, Ez0 (6-49) 


trong đó: K; là ma trận đối xứng dương. 

Thành phần thứ nhất của (6-49) đặc trưng cho động năng của hệ thống và 
thành phần thứ hai của (6-49) biểu thị thế năng tích lũy trong hệ thống có độ 
cứng là K;. 

Đạo hàm (6-49) nhận được: 


Ẫ 1 +. — 1 = * —G: 1. —.= 1 a 1 = 
V=~q H()q+~ä'H(q)d+—4TH(q)Ñ+-#TK,ẽ+—ẽTK,š 
1 (q)q 21 H)q 21 H()q NI nến 
Do q, là hằng số nên: £ = -3. Ma trận H(q) là ma trận đối xứng dương, 
do đó phương trình sau được thỏa mãn: 
q”H(8)ä = q"H(q)q (6-50) 
Sử dụng các ràng buộc trên, đạo hàm bậc nhất của V được viết ở dạng sau: 
x 1 =_- x —\— ~— — . _~ 
V= =4 H48 +4 H8 -4”K;ẽ (6-51) 
Từ phương trình động lực học robot (4-30b), rút ra thành phần mômen 
quán tính: 
H(4)8 = M - C(g,q)q—G(q) (6-52) 
Thay (6-52) vào (6-51) và thêm và bớt thành phân q”Kpg (với Kp là ma 
trạn hệ số đương đối xứng), (6-51) được viết ở dạng: 
Ÿ = L§gTHrayẻ —T M-C(a.#ì#—-G(a _T —.„. ST 4_zT + 
=3 H@)q+g [ (g.4)q—G(4)]—d K;ẽ + g`Koq-dgˆKpq 
1 _ ` h7 _—¬.- = 3... 1i — c — _— —~ 
=2'[H@)~2C(4,q)JÑ + 4"[M~ G(4)~K,ẽ+ Kuä]—äT Ksä 


(6-53) 
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Theo thuộc tính của phương trình động lực học ([6]): 
H(4)—2C(q.đ) =0 (6-54) 


Nếu chọn: M=G(q)+ KpEe-— K;ä „ Sử đụng (6-54) sẽ nhận được dạo 
hàm bậc nhất của hàm V âm: 
W=-q'K;q<0 (6-55) 
Với các điều kiện (6-49) và (6-55), hệ thống sẽ ổn định tuyệt đối xung 
quanh điểm cân bằng £ =0 tức là: q = qụ 
Như vậy luật điều khiển bù trọng lực có dạng: 
Mạ, =G()+K,E- KoÑ (6-56) 
Nhận thấy rằng, cấu trúc bộ điều khiển (6-56) gồm hai thành phần: Thành 
phần thứ nhất là trọng lực sẽ có tác dụng bù trọng lực của robot. Thành phần sau 
có cấu trúc PD (tỷ lệ - đạo hàm) tương tự như (6-I5a). Cân bằng hai phương 
trình mômen khớp robot (4-30b) và phương trình bộ điều khiển (6-56): 
H(q)Š + C(4,4)4 + G(4) = G(4) + K;ẽ - Koq (6-57) 
Ở chế độ làm việc xác lập, các thành phần tốc độ và gia tốc bằng không, từ 
phương trình (6-57) nhận được: 
K;pe=0 
Do đó, sai lệch vị trí của khớp robot luôn luôn bằng không tức là: 
g=q4, (6-58) 
Với luật bù trọng lực phi tuyến với bộ điều khiển PD (6-56), tất cả các 
điểm cân bằng (điểm làm việc xác lập) sẽ ổn định tuyệt đối không chịu sự ảnh 
hưởng của trọng lực của robot. Mức độ ổn định và chất lượng quá trình động 
phụ thuộc vào giá trị của K; và K. 
Ví dụ 6.3: Thiết kế bộ điều khiển bù trọng lực với bộ điều khiển cấu trúc 
PD (tỷ lệ - đạo hàm) cho robot hai thanh nối trong mặt phẳng có cấu hình như 
hình 6.10 với các tham số như ví dụ 6.I: 
m1 = 2,5 kg; m2 = 1,5 kg; mt = 0,5 kg; al = 0,25 m; a2 = 0,15 m 
+ Hệ phương trình động lực học robot có dạng tổng quát như (4-73): 
M=H(§)§ + V(4,q) + G(8) - (6-59) 
Các ma trận thành phần có dạng (6-25). 
+ Luật điều khiển có dạng (6-56): 
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My, =G(đ)+K,ẽ - Kzä (6-60) 
+ Quỹ đạo đặt cho hai khớp có đạng 2-1-2 được tính toán đảm bảo tay 


robot di chuyển từ vị trí ban đầu (0,2 0) đến vị trí cuối cùng (0 0,1) trong thời 
gian 2 s. 


+ Các hệ số khuếch đại tỷ lệ và đạo hàm được lựa chọn: 


2,65 
Kẹ;p= tệ ' và Kp= cá 
0 32/65 0 8 


Hình 6.14 là các kết quả mô phỏng. góc quay các khớp 1,2; sai lệch góc 
quay 1,2 và mômen khớp L,2. 


2¡~-———- : 








Ị „ 3/2: và: 
= : ng 4: : 
Là : Ỉ ị ¬- 
Ễì ; ¡ 8 : : 
= t2 2 œ 28: cả Ị 
Sở l8 Ðc ng 
Ø ạỊ... .} vá ị Ð.ố6152 si vì are bi 
Š " ỹ Ša+a. : 
LG Su meek-ee ce-l bi... 
9 1 2 3 4 0 1 2 3 3 


Thoi gian (s) Thơi gian (s} 








c1 HC EAc hoc ". TỶ. 
š 8ã vT hug $Ệ 1 .. khop 1 
> 8.3|- s : 2 „¡ : 
PP tp 2 2 By pH 
‡ Bh + ; 
Š°Ê| j khop2Ì Su 2 
“= xả Ỳ Ề 0; ¬... “= 
hi ớờnn XÀ § . ..khop # 
ND : 
ỗ gk—.. Bi sống dt để d6 ut =e, Š z2 xi s4 )2i Pha =—— 
9 1 2 3 4 0 † 2 3 4 
Thơi gian (s) Thơi gian (s} 


tlình 6.14. Kết quả mô phỏng của ví dự 6.3. 


Nhận xét: Hệ thống điều khiển với bộ điều khiển bù trọng lực với cấu trúc 
PD, sai lệch vị trí ở trạng thái xác lập bằng không; tuy nhiên chất lượng quá 
trình quá độ kém hơn các hệ thống điều khiển trên; 


6.4. Hệ thống điều khiển thích nghỉ 


Các phương pháp điều khiển chuyển động được trình bày ở các mục trên 
yêu cầu một mô hình động lực học robot chính xác và các tham số của robot 
phải được biết chính xác. Tuy nhiên, một số tham số robot khó có thể đo hoặc 
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xác định chính xác hoặc một số tham số biến đôi trong quá trình làm việc như 
khối lượng tải robot gắp ở tay, mômen quán tính tải, các thành phần ma sát 
trong các khớp robot... Với các bộ điều khiển kinh điển khó đạt được độ chính 
xác chuyển động. đặc biệt với robot hoạt động tốc độ cao. Các hệ thống điều 
khiển thích nghi được xây dựng sẽ đáp ứng được độ chính xác chuyển động khi 
tham số robot không được xác định chính xác hoặc biến đổi. 

Hai phương pháp điều khiển thích nghi được ứng dụng trong robot công 
nghiệp: Điều khiển thích nghỉ theo mô hình chuẩn và điều khiển thích nghỉ tự 
chỉnh. Nội dung của phương pháp điều khiển là chuyển động của robot được 
điều khiển bám theo chuyển động của một mô hình chuẩn (mô hình mẫu thiết 
kế trước) với các chỉ tiêu chất lượng mong muốn. Sơ đồ khối của hệ thống điều 
khiển theo mô hình mẫu có dạng ở hình 6. 15. 






e() 






Thích nghi 
tham số 






y() 
u() 


Luật thích nghỉ 


Hình 6.15. Sơ đồ khối hệ thống điều khiển thích nghỉ 
theo mô hình mẫu. 


Tín hiệu vào uŒ) được xác định từ khâu tính toán quỹ đạo chuyển động. Sai 
lệch giữa đầu ra của mô hình mẫu và đối tượng điều khiển e(t) là tín hiệu vào 
của khâu tính luật thích nghi tham số. Đầu ra khâu luật thích nghi tham số có 
thể là hai dạng: tín hiệu bù thích nghỉ g() hoặc tín hiệu chỉnh thích nghi tham 
số. Luật thích nghỉ tham số được thiết kế sao cho đảm bảo sai lệch giữa đầu ra 
của mô hình mẫu và đối tượng điều khiển e(1) luôn bằng không, tức là đại lượng 
đầu ra đối tượng luôn bám đại lượng đầu ra của mô hình mẫu. 

Sơ đồ khối của hệ thống điều khiển tự chỉnh được trình bày trên hình 6. 16. 
Ở hệ thống điều khiển này, tham số của robot được xác định (nhận dạng - ước 
lượng) và được sử dụng trong tính toán các tham số bộ điều khiển. 
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Chỉnh định Ước lượng 
tham số tham s 
Bộ ở 
s *Í điều khiển Đối tượng 


Hình 6.16. Sơ đồ khối hệ thống điều khiển thich nghi tự chỉnh. 






Q¡ 





6.4.1. Hệ thống điều khiển thích nghỉ theo mò hình mẫu 


Hệ thống điều khiển thích nghỉ theo mô hình mẫu được xây dựng cho một 
khớp robot truyền động bởi động cơ một chiều với giả thiết bỏ qua điện cảm 
phần ứng động cơ có sơ đồ cấu trúc như hình 6.17. Hệ truyền động một chiêu 
kinh điển của khớp robot gồm bộ điều khiển tốc độ với tín hiệu phản hồi tốc độ 
được lấy từ máy phát tốc tương tự hoặc máy phát tốc xung. Lượng đặt vị trí 
khớp robot 9„(t) được so sánh với vị trí thực của khớp robot Ô(1) tạo sai lệch e(1). 
Tín hiệu g(Đ) được bổ sung vào hệ thống nhằm đảm bảo sai lệch giữa mô hình 
mẫu và mô hình thực e(t) luôn bằng không khi tham số của robot thay đổi trong 
quá trình làm việc. 





Hình 6.17. Sơ đổ khối hệ thống điều khiển thịch nghi hệ 
truyền động khớp robot theo mô hình mẫu. 


Các tham số trong sơ đồ cấu trúc hình 6.17 là: 
] - mômen quán tính tổng của hệ thống qui đồi về trục động cơ; 
B - hệ số ma sát nhớt của động cơ; 
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+ - hệ số phản hồi tốc độ động cơ truyền động khớp; 
Ì - tỷ số truyền của bộ truyền động khớp; 
K¿p - hệ số khuếch đại của bộ khuếch đại; 
Kẹ, - hệ số biến đổi vị trí/điện áp. 
Sơ đồ cấu trúc hình 6.17 được biến đổi thành sơ đồ hình 6.18. 





Hình 6.18. Sơ đồ khối hệ thống điều khiển thích nghỉ 
hệ truyền động khớp robot theo mô hình mẫu. 


Trong sơ đồ hình 6.18: Ko= KgpK,, 
Từ sơ đồ cấu trúc hình 6.18, sau một số biến đổi, viết được phương trình 
Sau: 
RM, 


mỶ^Ó 





(p° +2š@„p + œ2 )Ô+(p) =@2Ôr„(p) + G2 l9 | (6-61) 


“Tn - hệ số suy giảm; 
œ,„ =./KoK„/RJi - tần số dao động riêng. 

Phương trình (6-61) là phương trình bậc 2. Trên cơ sở đó, chọn mô hình 
mẫu có dạng bậc 2: 

(pỶ +a„„p + bạ )Õ„ (p) = b,„Ð,„¿(P) (6-62) 

Các hệ số a„. và b„ của phương trình (6-62) được lựa chọn theo điều kiện ổn 
định và hội tụ của mô hình mẫu với các chỉ tiêu động mong muốn: độ quá điều 
chỉnh và thời gian quá độ. 

Tín hiệu bổ sung g(p) sẽ được xác định sao cho tổng g(p) + e{p) là tín hiệu 
vào hệ thống truyền động sẽ đảm bảo góc quay khớp robot luôn bám đầu ra mô 
hình mẫu. 

Đặt sai lệch giữa đầu ra mô hình khớp và đầu ra mô hình mẫu là: 

eœ) = 8„(p) - 8r(p) (6-63) 


trong đó: š = 
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Kết hợp hai phương trình (6-61) và (6-62) với sử dụng định nghĩa (6-63) 
phương trình động học sai số của hệ thống có dạng: 





R 
(pŸ + amp + bạ, )e(P)= (bạ — 02 )£(p) + (2š, — a„,)pÔ+(p) + G2 Ễ x - gí9| 
m'®*O 
(6-6%) 
Phương trình (6-64) được viết ở dạng biến thời gian ở đạng: 


+a„e+ bạ =(b„ —@2)£(t)+(2E6, —a„)ÔÖ+(Ð) + @Ÿ bà: " g0) (6-65) 
mo 


Để đảm bảo hệ thống ổn định và sai số hội tụ về không, tín hiệu g(£) có thể 
được lựa chọn gồm ba thành phần như sau: 


g( = g,(Đe(Ð + g;(ĐÕr(t) + g;(Ð (6-66) 
Đặt biến trạng thái: 

X¡(Ð =e(Ð 

x;() =X⁄¡(Ð =&(9 (6-67) 


X=[x¡@,x;(ŸÏ 
Thay phương trình (6-66) vào phương trình (6-65) với sử dụng các ký hiệu 
biến trạng thái (6-67), nhận được phương trình trạng thái mô tả hệ thống như sau: 
XŒ)= A„X(Œ)+b,e(Ð + b;Ốy (Đ)+ bị (6-68) 
Các-ma trận của phương trình (6-68) được xác định theo các biểu thức sau: 


Wỉ. 8 an. 0 
Ni —bm Tâm : v1 bạ — G2 — B0. : 


, (6-69) 


„ 
n — #m — Bia (M;R/KaKo —8;)0®; 
Xác định các hàm g,(), g;(Ð và g;(Ð đựa trên tiêu chuẩn ổn định Liapunov. 
Chọn hàm Liapunov có đạng sau: 
Vị = XTPX+a,blb, +a;bb; +asbjb; (6-70) 
trong đó: P - là ma trận đối xứng dương; 
ä¡, ;, a; - các hệ số dương. 
Với các hệ số đã định nghĩa, (6-70) cho thấy rằng hàm V, >0 với X, bị, b; 
và bạ z 0. 
Đạo hàm hàm V, và sử dụng (6-68) ta nhận được biểu thức sau: 
Vị =XT(PA„ + ATP)X+2XTPb,e+2XTPb,Ôy +2XTPb, 62) 
+2a,b†b, +2a;B;b„ +2a;Ð; bạ 
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Từ (6-71), nếu chọn các luật thịch nghị có dạng sau: 


bị =—a!XTPE (6-72a) 
bộ =~a;'XTPôy (6-72b) 
bị =-a;!'XTp (6-72c) 
và: PA„a+AnP=-Q ; Q=Q>0 (6-73) 


sẽ nhận được: Ÿ, =XT(PA„+ATP)X=-XTQX với X+0. Theo điều kiện ổn 
định Liapunov, hệ thống sẽ ổn định tuyệt đối xung quanh điểm cân bằng ổn 
định X=0. 

Với giả thiết các tham số khớp robot biến đổi chậm, từ (6-72) và (6-69), 
các thuật toán xác định hàm g;(t), ø;() và g() nhận được ở dạng sau: 


6i =Y,(XTP);e (6-74a) 
&; =Y;(X"P);Ôr (6-74b) 
&› =Y;(X TP); (6-74c) 


trong đó: y¡, Y„, ÿ; - các hằng số dương, được chọn bằng phương pháp mô phỏng; 
(XTP), - thành phần cột 2 của ma trận (XP) được xác định theo (6-78). 





Hình 6.19, Sơ đồ khối hệ thống điều khiển một khớp theo mô hình mẫu. 
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Trong thiết kế cụ thể, có thể chọn Q, yạ, Y;, yạ sao cho đạt được mức độ hội 
tụ nhanh nhất. Sơ đồ khối chỉ tiết của hệ thống điều khiển được trình bày trên 
hình 6.19. Tín hiệu g(t) được xác định theo biểu thức (6-64) với sử dụng (6-74) 
có dạng: 


+ † { 
g(t) =Tie() [CX”P);e(0d+ + v;ô,(Ð [OfTP);ô(Đdr + y; [CTP);dr — (6-75) 
0 0 0 


Tóm tắt các bước thiết kế luật điều khiển: 
(1) Chọn mô hình mẫu (a„ và bạ). 
(2) Chọn các hệ sỐ Y¡, ÿ;, ya. 
(3) Chọn ma trận Q. Giải phương trình (6-72) xác định được các thành 
phần của ma trận P; 
Pụ; = 0,5y/bạ và Đạ; = 0,5; + Yu/ba)/4„ (6-76) 
(4) Xác định các thành phần của X và (XTP),: 





X.=Ô„- Ôy VÀ x; =Ô, -Ôy (6-77) 


CXTP); =x¡Bạ +x;P;; (6-78) 

(5) Xác định g() theo biểu thức (6-75). 

6.4.2. Hệ thống điều khiển động lực học ngược thích nghỉ 

Hệ thống điều khiển động lực học ngược (mục 6.3.2) đã được thiết kế trên 
cơ sở tất cả các tham số robot được xác định chính xác. Nếu giá trị các tham số 
sử dụng trong tính toán bộ điều khiển khác với giá trị thực của các tham số của 
robot, tính phi tuyến và sự ràng buộc của hệ thống động lực học không được 
khử hoàn toàn, độ chính xác điểu khiển sẽ giảm. Bộ điều khiển động lực học 
ngược thích nghị được thiết kế cho robot với mục đích đảm bảo khử hoàn toàn 
tính phi tuyến và ràng buộc của hệ thống trong trường hợp các tham số robot 
không được xác định chính xác. 

Bộ điều khiển động lực học ngược thích nghỉ sẽ được xây dựng trên cơ sở 
của luật điều khiển động lực học ngược (6-27) với các tham số sử dụng trong 
tính toán luật điều khiển được nhận dạng bởi khâu nhận dạng online tham số 
robot. Khi đó phương trình luật điều khiển với tín hiệu điều khiển phụ có cấu 
trúc PD được biểu diễn ở dạng sau: 

Mạ = H(q)(8 + KoŠ + KạE) + Ÿ(4,8) + (4) (6-79) 


trong đó: Hq), V8), ö(8) - các ma trận ước lượng của các ma trận H(), 
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V(4.q), G(4), được tính toán từ các tham số được nhận dạng trong 
quá trình làm việc; 
E=q¿—q - vectơ sai lệch vị trí khớp robot; 
K;, K; - các ma trận đường chéo. 
Sơ đồ khối của hệ thống điều khiển có dạng như hình 6.20. 


ÐI 


bị: 


J.. 


Luật 
thích nghi 





Hình 6.20. Sơ đồ khối hệ thống điều khiển động lực học ngược thích nghi. 

Cân bằng đầu ra bộ điều khiển (6-79) và đầu vào mô hình robot (6-14), với 

sử dụng các ký hiệu đã nêu, nhận được phương trình sau: 
H()(ẽ + Kpẽ + K;£) = ñ(4)8 + Ÿ(,q) + õ() (6-80) 
trong đó: (4) = H(q)~ Ñ(4) - ma trận sai lệch ước lượng của H() ; 
Ÿ(q,g)= V(4.q)- Ÿ(4,8) - ma trận sai lệch ước lượng của V(,8); 
(8) = G(4)— Ö(4) - ma trận sai lệch ước lượng của G(8). 
Phương trình (6-80) được viết lại ở dạng sau: 
š+K,£+K;E=ñ"!4)M (6-81) 
trong đó: M = Í(8)ä + Ÿ(,4) + Õ(4) - vectơ sai lệch mômen gây ra bởi sai lệch 
nhận dạng tham số. 

ý nghĩa của phương trình (6-81): Phương trình biểu thị quan hệ giữa sai số 
điều khiển (sai số vị trí khớp E ) và sai số nhận dạng tham số ( M ). Khi các tham 
số robot được ước lượng chính xác, tức các ma trận sai lệch HÍ(q), Ÿ`(q, 9®. 
Õ() đều bằng không, phương trình (6-81) sẽ có dạng: 

£+KpE+K;E=0 (6-82) 
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Điều đó có nghĩa là có thể tính toán được Kạ, Ky để sai số điều khiển hội tụ 
về không với tốc độ hội tụ mong muốn. 

Sử dụng thuộc tính tuyến tính của phương trình động lực học robot ({6]), 
vectơ sai lệch mômen có thể phân tích thành hai thành phần và phương trình 
(6-81) được viết ở dạng sau: 

+ K;ẽ + K;ẽ =ñ-'q)Ww(,á,a).Đ (6-83) 
trong đó: W(,q,q) - ma trận (nxr) là hàm chứa các tham số đã biết; 


Š- vectơ (rx L) chứa các sai lệch của tham số robot chưa biết cần ước lượng. 
Từ phương trình (6-83), phương trình sai số của một khớp được viết ở dạng 
Sau: 


š, + kụg; + k„e; =[ Ñ'4)W(8,3,a) 6 | (6-84) 


J 


trong đó: [ñ'awdi.a.aẽ | - thành phần hàng j của ma trận 


j 
ñ'q)Wd,3.a)6. 
Đặt biến trạng thái: X;¡ =[x¡,x;;]Ï =[e;,ẻ,]Ÿ - vectơ trạng thái sai lệch của 
khớp j và X =[X,X¿,.....X„]Ï - vectơ trạng thái sai lệch của robot. 
Phương trình trạng thái của một khớp j sẽ có dạng: 
Š,=A#,+B,| Ñ !(4)W(,8,a)5 | (6-83) 
J 
. 0 1 0 
trong đó: Á,=  - ; B,= ' 


Đ 
Sử dụng (6-85), hệ phương trình trạng thái chung cả hệ thống robot sẽ có 
dạng: 


X=AX+ BÑ"'(4)W(ä,ä,q)Š (6-86) 
Ai 0. 0 B0... 0 

trong đó: A= ; A; nó : se| B; „ : 
m..: 0 0 2B, 


Thuật toán nhận dạng thích nghỉ được xây dựng sử dụng tiêu chuẩn ổn định 
Liapunov. Chọn hàm Liapunov có dạng: 
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Vị =XTPK +@TT"!Ã > 0 (6-87) 
trong đó: P - là ma trận đối xứng đối xứng dương; 
I"` - ma trận đường chéo đương. 
Đạo hàm hàm V, và sử dụng (6-86), sau một số biến đổi nhận được: 


Ÿ.Ø@ð) =-XTQX +28@(WTR-'BTPK +Tˆ!6) (6-88) 
Nếu chọn: I”'ð =~WT(3,,q)Äˆ'(q)BTPX (6-89a) 
Và: PA +ATP=-Q ; Q'=Q>0 (6-89b) 


sẽ nhận được: V„ =—XTQX <0 với X z 0. Theo điều kiện ổn định Liapunov, hệ 

thốnÿ sẽ ổn định tuyệt đối xung quanh điểm cân bằng ổn định X =0. Vectơ sai 

số tham số là hiệu của vectơ tham số thực và vectơ tham số ước lượng: 
$=8-ô (6-90) 

Với giả thiết các tham số khớp robot biến đổi chậm, luật nhận dạng thích 
nghi - nhận được bằng kết hợp (6-89a) và (6-90): 

°=TWT(8,ä,ĐÑ '4)B”PX (6-91) 

Ví dụ 6.4: Xây dựng hệ thống điều khiển động lực học thích nghỉ cho robot 
2 thanh nối có hình 6. 10. 

+ Hệ phương trình động lực học robot có đạng tổng quát như (4-73): 

M=H()4 + V(,q) + G(4) 

Phương trình động lực học của robot 2 thanh nối có dạng như (6-24) và 
(6-25): 

Viết phương trình (6-24) + (6-25) với tách thành các thành phần chứa hai 
biến m, và m; là các tham số chưa biết cần phải được nhận dạng trong quá trình 
làm việc? 

M, = w¡m; + W;;m; (6-92a) 
M¿ =w;imm; + w;;m; (6-92b) 
trong đó: w¡; =aj +ga,Cl 
Wị, =[(a} +4) +2a,a;C2)Ö, +(a; + a¡a;C2)Ö; 
~a,a,S2(20,Ô; +67) + g(a,CI+ a,C12)] 
wại=0 


wị; =[@j +a,a,C2)Õ, +a;Ö, +a,a;S262 + ga,C12)] 
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mị và m; là các tham số cần ước lượng. Các ma trận wạ;, Wịz, W;, và w;; được 
tính từ các tham số đã biết. Từ các phương trình (6-92), phương trình sai lệch 
mômen được viết ở đạng sau: 

Mì =Mi —Ñg = WijÑN + Wiyñ; 


M; =M;T—M; =w¿¡ñ + w;;mn; 


Hoặc ở dạng ma trận: 
ÂM v5 là |> Mổð„ô,6,0,s)ỗ 6-93) 
Wại W¿; mm; : 


= m SA 4 
với Œœ "| : | - Vectơ sai lệch tham số ước lượng. 


m; 
Áp dụng (6-91), luật nhận dạng thích nghỉ cho robot 2 thanh nối có đạng: 


&_ | T2 + =f-l—anTp% 
®=|, |=IW (q,qq)H (q)B PX (6-94) 
m; 
Ma trận W(q,đ.q) được xác định theo (6-93); Ma trận hệ số quán tính hiệu 


quả được viết ở dạng: 


ñ -lh" Si &È 
Hạ H, 
VỚI: 
ñ =1? +ửn,(a? +a) +2a,a, cos6, (6-96a) 
Ñụ Š ñ, = t,[a? +aiâ; cos6, } (6-96b) 
ñ„ = fh;a? (6-96c) 


Luật điều khiển động lực học ngược thích nghi có dạng (6-79): 
Mụ, =Ñ{@)(8; + K;ẽ + K;E)+ Ÿ(8,8) + (4) 


6.5. Hệ thống điều khiển trong không gian làm việc 

Trong các hệ thống điều khiển không gian khớp được trình bày ở các mục 
trên, tín hiệu đặt là quỹ đạo chuyển động biểu diễn vị trí khớp theo thời gian, sai 
lệch điều khiển là sai lệch vị trí khớp. 

Trong thực tế, chuyển động của robot được đặt trong không gian làm việc 
(không gian Đecac), đo đó thuật toán động học ngược cần thiết để biến đổi quỹ 
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đạo đặt trong không gian tay về không gian khớp. Điều đó sẽ làm tăng khối 
lượng tính toán. Vì vậy, hệ thống điều khiển robot công nghiệp thông thường sẽ 
tính vị trí của khớp thông qua bài toán động học ngược, và sau đó tính toán tốc 
độ và gia tốc khớp bằng phương pháp vi phân số. 

Ở hệ thống điều khiển không gian làm việc, tín hiệu đặt trực tiếp là quỹ đạo 
chuyển động mong muốn của tay robot trong không gian làm việc, lượng phản 
hồi sẽ được tính từ vị trí của khớp thông qua khâu động học thuận. Khâu tính 
động học ngược được cài đặt trong mạch vòng điều khiển phản hồi sẽ tính đối 
các biến về không gian khớp. Yêu cầu về thời gian mẫu sẽ làm giảm chất lượng 
của hệ thống. 

Vì những hạn chế nêu trên, hệ thống điều khiển không gian làm việc chỉ 
được sử dụng hiệu quả khi thực hiện điều khiển tương tác giữa tay robot và môi 
trường làm việc. Thực tế, hệ thống điều khiển không gian khớp được thiết kế 
cho điều khiển robot khi chuyển động tự do trong không gian. Khi tay robot 
hoạt động trong điều kiện ràng buộc với môi trường, ví dụ: tay robot tiếp xúc 
với môi trường làm việc, cần thiết điều khiển đồng thời vị trí và lực tiếp xúc. 
Trên quan điểm đó, trong mục này sẽ trình bày về các phương pháp điều khiển 
đơn thuần về chuyển động tự do của robot trong không gian làm việc. Vấn để 
điều khiển tương tác giữa tay robot và môi trường sẽ được trình bày ở chương 7. 

Sơ đồ khối chung của hệ thống điều khiển chuyển động trong không gian 
làm việc như ở hình 6.2. Lượng đặt của hệ thống điều khiển là quỹ đạo chuyển 
động của tay robot. Sai lệch giữa vị trí đặt của tay và phản hồi vị trí tay ở không 
gian tay sẽ được chuyển đổi thành mômen hoặc lực của khớp truyền động cho 
các khớp. Hai hệ thống điển hình là: hệ thống điều khiển ma trận Jacobien 
chuyển vị và hệ thống điều khiển ma trận Jacobien đảo. 


6.5.1. Hệ thống điều khiển ma trận Jacobien chuyển vị 
Lực cần thiết để di chuyển tay theo quỹ đạo đặt trong không gian làm việc 
được xác định từ sai lệch vị trí và sai lệch tốc độ trong không gian làm việc 
tương ứng với luật điều khiển phản hồi PD kinh điển: 
ö„ =K;(, -S)+K(S, -5) (6-94) 
trong đó: G„„ - vectơ lực cần thiết để tay robot đi chuyển theo quỹ đạo và tốc độ 
đặt trước; 
S¿, S - tương ứng là vectơ vị trí đặt và vectơ vị trí thực của tay robot; 
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Sr) s= tương ứng là vectơ tốc độ đặt và vectơ tốc độ thực của tay 
robot; 
K; = diag(K„¡, K,a„..., K2) - ma trận đường chéo các hệ số khuếch đại; 
"Ko = diag(Kui, Kạ,...., K„¿) - ma trận đường chéo các hệ số đạo hàm. 

Vectơ lực ở tay robot được biến đổi về lực hoặc mômen khớp robot thông 
qua ma trận lacobien chuyển vị (4-102). Khi đó, kết hợp với (6-94), vectơ 
mômen truyền động khớp robot được xác định như sau: 

Mạ, =JT[K;(, -§)+Kp(Š, ~Š)] (6-95) 

Sơ đồ khối hệ thống điều khiển vị trí tay robot sử dụng ma trận Jacobien 
chuyển vị trình bày trên hình 6.21. Tín hiệu phản hồi vị trí tay robot được tính 
từ vectơ vị trí khớp bởi khâu động học thuận (ĐHT). Vectơ tốc độ thực của tay 
robot được tính từ vectơ tốc độ khớp robot thông qua ma trận Jacobien. 

Ví dụ 6.5: Thiết kế bộ điều khiển vị trí của tay robot với luật điều khiển ma 
trận Jacobien chuyển vị cho robot hai thanh nối trong mặt phẳng có cấu hình 
như hình 6. LŨ với các tham số như xí dụ 6.I: 

ml = 2,5 kg; m2 = 1,Š kg; mt = 0,5 kg; a1 = 0,25 m; a2 = 0,15 m 
+ Hệ phương trình động lực học robot có dạng tổng quát :..:ư (4-73): 
M=H()đ + V(,3)+G() (6-96) 
+ Luật điều khiển ma trận Jacobien chuyển vị có dạng như (6-95). 
+ Hệ số K; và Kp được lựa chọn như sau: 


50000 5000 
K,= K¬= 
: 0 oi "Hiến _ bị 


Kết quả mô phỏng với quỹ đạo chuyển động dạng 2-1-2 được trình bày trên 
hình 6.22. 





Hinh 6.21. Sơ đồ khối hệ thống điều khiển ma trận Jacobien chuyển vị. 
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Hình 6.22. Kết quả mỏ phỏng của ví dụ 6.5. 


6.5.2. Hệ thống điểu khiển ma trận Jacobien nghịch đảo 
Phương pháp điều khiển ma trận Jacobien nghịch đảo được xây dựng trên 
cơ sở của phương pháp điều khiển ma trận Jacobien chuyển vị. Như (6-94), sai 
lệch vị trí và robot sai lệch tốc độ của tay robot được định nghĩa tương ứng là: 
8S =S—Š (6-96a) 
8§=S-Š (6-96b) 
Khi coi các sai lệch vị trí và sai lệch tốc độ tay robot là các đại lượng nhỏ, ' 
sử dụng biểu thức quan hệ giữa các vectơ dịch chuyển nhỏ của tay và khớp hoặc 
quan hệ tốc độ tay và khớp (3-22), vectơ sai lệch vị trí và sai lệch tốc độ của 
khớp được xác định bởi các phương trình sau: 
£=õq=J"'ôS (6-97a) 
# =ðä =J!8S (6-97b) 
trong đó: J” - ma trận Jacobien robot nghịch đảo. 
Tương tự như luật điều khiển phản hồi PD kinh điển (6-16), vectơ mômen 
cần thiết truyền động cho khớp robot được xác định theo phương trình sau: 
Mạ =KpJ'{§, —8)+KpJ'(6, —S) (6-98) 
với K; và Kp là các ma trận đường chéo. 
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Do tính đối xứng của hai ma trận hệ số K; và K›, phương trình (6-98) được 
viết lại ở dạng: 


Mạ=1" [kuG, ~§)+Ku( -Š| (6-99) 


Sơ đồ khối của hệ thống điều khiển ma trận Jacobien nghịch đảo được biểu 
diễn trên hình 6.23. 





Hinh 6.23. Sơ đồ khối hệ thống điều khiển ma trận .Jacobien đảo. 
Bài tập 

6.1. Thiết kế hệ thống điều khiển cho hệ truyền động khớp độc lập với ba mạch 
vòng điều chỉnh: Gia tốc, tốc độ và gĩa tốc có tham số sau: R_= 0,3 Q; =6 
kgm?; K„ = 0,5 Nm/A; K, = 0,5 V/rad/s đảm bảo chỉ tiêu chất lượng hệ 
thống kín: Độ suy giảm š > 0,4 và tần số tự nhiên œ„ = 20 rad/s 

6.2. Một robot 2 thanh nối có tham số: a; = a;= Ím; mị = m; = 50 kỹ; ], = ]; = 
10 kgm?. 

Động cơ có tham số: Rụ;= R„ = 10 ©; K„¡ = K„¿= 2 Nm/A; 
K.¡ = K¿ = 2 V/rad/s. 
Thiết kế bộ điều khiển bù trọng lượng với luật PD. 

6.3 Một robot 2 thanh nối như 6.2. Thiết kế hệ thống điều khiển động lực học 
ngược. Mô phỏng hệ thống khi khớp robot di chuyển từ vị trí ban đầu [0, 
1/4] đến vị trí cuối cùng [2/2, x/2]T với đồ thị tốc độ hình thang trong thời 
gian 4 s. 

6.4 Một robot có các tham số như 6.2. Thiết kế hệ thống điều khiển phản hồi 
PD cho các khớp. Mô phỏng hệ thống khi khớp di chuyển theo quỹ đạo như 
bài 6.3. 

6.5. Thiết kế hệ thống điều khiển vị trí tay sử dụng ma trận Jacobien đảo và 
chuyển vị cho các khớp của robot 2 thanh nối có các tham số như 6.2. Mô 
phỏng hệ thống khi tay robot di chuyển theo quỹ đạo hình thang từ vị trí 
ban đầu [0,1] m đến vị trí cuối cùng [1,0] m trong thời gian 4 s. 
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Phụ lục 
Chương trình mô phỏng MATLAB cho ví đụ 6.1 


6.1. Eile chương trình mô phỏng 


4Chuong trinh mo phong he thong dieu khien vi tri robot 2 thanh noi 
% Controller2: Bo dieu khien vị tri voi bo dieu khien phan hoi PD 
%4x11=x0(1);#eGoc theta Í 
%xI2=x0(2);%Toc do Goc khop Ï 
%4x2!=x0(3);%Goc theta2 
%4x22=x0(4);2Toc do Goc khop 2 
tc=2; 
%Vi tri tay ban dau va cuoi 
S0={0.2,0]; 
Sc-={0,0.1]; 
%‹2Vi tri khop ban đau va cuoi 
qo=DHN(S0); 
qc=DHN(Sc); 
%Gia toc hai khop 
ddq1=1.3*4*abs(qc(1)-qo(1))/tc^2; 
ddq2=1.3*4*abs(qc(2)-qo(2))4c^2; 
%Cac khoang Thoi gian 
tÍ1=tc/2-sqrt((tc^2*ddq1-4*(qc(1)>qo(1))/dda1)/2; 
2l=tc-tl l; 
t12=tc/2-sqrt((tc^2*ddq2-4*(qc(2)-qo(2))/ddq2X⁄2; 
t22=tc-tl2; 
=0.0]; 
%sDieu kien dau 
q=qo;dq=[{0;0]; 
X0={[qo(I);dq(1);qo(2);dq()]; 
filel=fopen(tdata2.$',"w'); 
for t=0:0.001:1c; 
[qd1,dqd1]=quidaokhop2(qo(1),qc(1),đdq1,t11,21,tc,f); 
[qd2,dqd2]=quidaokhop2(qo(2),qc(2),ddq2,t!2,122,tc,t); 
qd=[qd1;ad2); 
dqd={dqd!;dqd2]; 
{M,xe,fe]=Controller2(qd,dqd,q,dq); 
[q,dq]ERobot3(M,X0,Tk); 
X0={a(1);dq(1);q(2);dq(2)]; 
fprintf(file1,%2.4f %2.4f %2.4f %2.4f %2.4f %3.4f 
%3.4fn',t,qd(1),q(1),qđ),q(23)M(1),MQ)); 
end . 
fclose(file ]); 


2. Hàm tính luật điều khiến 
ñunction [M,errortheta,errordtheta]=Controller2(sd,dsd,theta,dtheta) 
%Bo dieu khien vi trị phan hoi PD 





%Cac thong so cua Robot 
m1=2,5;%Khoi luong thanh I 
m2=1.Š;%sKhoi luong thanh 2 
mt=0.5; %Khoi luong tai 

al =0.25;%Chieu đai thanh I 
a2 =0. I5; %aChieu đai thanh 2 


?4tinh sai lech goc quay va dao ham goc quay 
errortheta=sd-theta; 

errordtheta=dsd-dtheta; 

%Mo men đieu khien 

Kp=[{kpI,0;0,kp2], 

Kd=Ikd1,0;0,kd2); 
M~=Kp*errortheta+Kd*errordtheta; 


3. Hàm tính lượng đặt: 
function [q,dq]=quidaokhop2(qo,qc,ddq,t1,t2,tc,t) 
%Chuong trính tỉnh qui dao 2-1-2 va toc do khop (do thí hình thang) . 


ift<=tI 
q=qo+ddq*t^2/2; 
dq=dda*t; 
elseif (t>tl) & (t<=12) 
q=qo+dda*tI*(1-11/2); 
dq=ddq*t1; 
elseif(tˆt2)&(t<=tc) 
q=qc-ddq*{(t-tc)^2/2; 
dq=dda*t1-dda*{t-t2); 
end 
end 


4. Hàm tính mô hình Robot 
function [q,dq]=Robot3(M,X0,Tk}) 
%Mo phong robot 2 thanh noi 


9⁄Ạ——=—============—=======~=========~z==>—====—======== 
Mol=M(1);%Mo men đieu khien khop] 
Mo2=M(2);%Mo men dieu khien khop2 


xÌ I=X0(1);%Goc thetal 
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x12=X0(2);%⁄Toc do Goc khop l 
x21l=X0(3);%⁄Goc theta2 
x22=X0(4);⁄2Toc do Goc khop 2 


9=========—=- —======———: ====—=——————=====——: —— 





%Cac thong so cua Robot 
m1=2.5;3sKhoi luong thanh I 
m2=1.5;%Khoi luong thanh 2 
mt=0.5; %Khoi luong tai 

al =0.25;%Chieu đai thanh I 
a2 =0, |5; %s4Chieu dai thanh 2 


94========= 
CI=cos(xI 1); 
C2=cos(x2l); 
CI2=cos(xI1+x2l); 

SI=sin(xI I); 

S2=sin(x2l}; 

S12=sin(xI1+x21); 

%Ma tran quan tính 
HI11=(m]I+m2)*a1^2+m2*a2^2+2?m2*a | *a2*C2; 
H12=m2*a2^2+m2*a1 *a2*C2; 

H22=m2*a2^2; 

H2I=HI2; 

H=[H11,H12;H21,H22]; 

#2Mo men nhot va huong tam 

v(1)=-m2*#al] *a2#S2*x22^2-2*m2*a1*a2*S2*x12*x22; 
v(2)ˆm2*a1*a2*S2*xI2^2; 

V=[v();v(2)], 

%2Mo men trong lục 

G]=(ml+m2)*9.8I *a1*CI+m2*9.81*a2*C12; 
G2=m2*9,81*a2*C12; 

G~[|GI;G2]; 

%sNgích dao ma tran H 

Hinv=inv(H); 

%sGia toc khop 

dX=-Hinv*(V+G)+Hinv*{[Mo1;Mo2]; 

%sPhuong trinh trang thai 

xIIp=x12; 

x2lp=x22; 

xI2p=dX(I); 

x22p=dX(2); 

%Tinh gan dung phuong trinh vi phan 
xI1=xI1+Tk*x1 1p; 

x21=x21+Tk*x2lp; 

x12=x12+Tk*x12p; 
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x22=x22+Tk*x22p; 


X={[xI1;xI2;x21;x22]; 

% Tỉnh vi trí tay robot 

Cl=cos(x1 1); 

CI2=cos(xII+x2l); 

SI=sin(x1 1); 

SI2=sin(xII+x2l); 
s=[al*CI+a2*C12;a1*SI+a2*S12]; 
q={xI1;x2H; 

dq=[{x12;x22]; 
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CHƯƠNG 7 
ĐIỀU KHIỂN LỰC 


Điều khiển lực là trường hợp tổng quát của điều khiển quỹ đạo, vì đại đa số 
robot trong quá trình làm việc sẽ tiếp xúc với môt trường khi di chuyển theo 
một quỹ đạo đặt trước. Khi đó cần thiết điều khiển cả vị trí robot và lực robot 
sinh ra. Ví dụ các robot lắp ráp, đánh bóng, đóng và mở cửa. Hai phương pháp 
điều khiển lực đã được phát triển là: Điều khiển trở kháng (điều khiển độ nhún) 
và điều khiển hỗn hợp. 

Nguyên lý cơ bản của điều khiển trở kháng là điều khiển vị trí và lực bằng 
điều chỉnh trở kháng (độ nhún) của cơ cấu cơ khí tay robot phù hợp với ngoại 
lực tác đụng lên tay robot khí tiếp xúc với môi trường. Trở kháng cơ khí là một 
khái niệm biểu thị độ cứng của cơ cấu chống lại lực tác dụng lên nó. Điều khiển 
trở kháng gồm hai phương pháp: Phương pháp điều khiển trở kháng thụ động và 
chủ động. Ở hệ thống điều khiển trở kháng thụ động, trở kháng cơ khí của bộ 
phận kẹp chỉ tiết của tay robot được điều chỉnh bằng các phần tử cơ khí như lò 
xo, bộ giảm chấn. Phương pháp điều khiển trở kháng tích cực thực hiện điều 
khiển trở kháng cơ khí theo hệ thống kín của cơ cấu chấp hành khớp với các tín 
hiệu phần hồi vị trí, tốc độ và lực... . 

Điều khiển hỗn hợp gồm hai kênh điều khiển độc lập vị trí và lực. Các tín 
hiệu đầu ra của bộ điều chỉnh sẽ điều khiển cơ cấu chấp hành khớp bám theo 
quỹ đạo vị trí và sinh ra lực đặt trước. Hướng vị trí và lực được điều khiển tuỳ 
thuộc vào nhiệm vụ robot thực hiện trong đây chuyền sản xuất. 


7.1. Điều khiển trở kháng 

7.1.1. Phương pháp điều khiển trở kháng thụ động 

Một tay robot có nhiệm vụ lắp một chốt vào một lỗ với độ kín khít rất cao 
hay độ rơ rất bé. Quá trình lắp ráp chính xác được thực hiện khi trục của chốt 
trùng khít với trục của lỗ. Khi đó tay robot chỉ cần sinh ra một lực rất nhỏ để 
thực hiện quá trình lắp ráp. Tuy nhiên, nếu có sự sai lệch về hướng trục của chốt 
và hướng trục của lỗ thì quá trình lắp ráp có thể không thực hiện được. Whitney 
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(1979) ([16]) đã đề xuất tay robot có “cơ cấu đàn hồi có tâm ở xa” (CCĐHTX). 
Tay robot dạng CCĐHTX có tính đàn hồi ở một vài hướng làm cho quá trình lắp 
ghép dễ dàng hơn, ví dụ: chỉ tiết được kéo vào lỗ chứ không bị đẩy vào lỗ. Cấu 
trúc động học của tay robot được trình bày trên hình 7.la. Hệ thống gồm 4 
thanh nối cứng S, — S„, mỗi thanh gồm 2 lò xo ở 2 đầu. Các lò xo bên ngoài chỉ 
bị bẻ cong mà không bị nén hoặc kéo dãn. Trục của các thanh nối S, và S; song 
song nhau, còn trục của thanh nối S; và S; cắt nhau tại một điểm O, (điểm cuối 
của thanh chốt) sao cho khi có một lực tác dụng lên điểm đó sẽ chỉ gây ra 
chuyển động tịnh tiến hoặc chuyển động quay xung quanh điểm đó khi có một 
mômen tác dụng lên điểm đó. 





Hình 7.1. Cơ cấu động học của tay robot dang CCĐHTX. 


Trường hợp trục của chốt và lỗ song song nhau nhưng không trùng khít 
nhau, tay robot chuyển động theo phương thẳng đứng về phía lỗ và chạm vào 
thành lỗ như hình 7.1b. Khi đó có một lực tác dụng lên thanh chốt theo phương 
nằm ngang như hình 7.1b và làm cho các lò xo §, và S„ bị bẻ cong còn các lò xo 
Š; và S; vẫn giữ nguyên trạng thái. Kết quả là thanh chốt sẽ dịch chuyển theo 
phương ngang như chiều mũi tên chỉ ra trên hình vẽ mà không bị thay đổi 
hướng và nó sẽ trượt vào lõ. 

Trong trường hợp trục thanh chốt không song song với trục lỗ, tay robot sẽ 
đẩy chốt vào lỗ cho đến khi nó bị mắc kẹt ở vị trí như hình 7.Ic. Dưới tác dụng 
của mômen tác dụng lên đầu chốt, lò xo S; và S; bị bẻ cong làm thay đổi hướng 
dịch chuyển của chốt về trùng với trục của lỗ. Như vậy chốt sẽ được lắp vào lỗ. 

Cơ cấu tay robot đạng CCĐHTX 3 chiều được vẽ trên hình 7.2. Tương tự 
như tay robot 2 chiều, các trục của lò xo vòng ngoài song song nhau, trục của 
__ các lồ xo vòng trong cắt nhâu tại điểm O,. 
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Phương pháp trở kháng thụ 
động không yêu câu mạch vòng 
điều khiển lực, do đó hệ thống điều 
khiển sẽ đơn giản. Tuy nhiên, cơ 
cấu robot sẽ không linh hoạt, vì với 
mỗi một nhiệm vụ khác nhau sẽ 
phải thiết kế một phần cứng khác 
nhau. Tay robot dạng CCĐHTX 
thường được sử dụng để cặp và lắp 
ghép chỉ tiết có chiều dài ngắn. Hình 7.2. Cơ cấu tay robot dạng CCĐHTX 3 chiều. 





7.1.2. Phương pháp trở kháng tích cực 

Phương pháp điều khiển trở kháng tích cực sẽ giải quyết được những hạn 
chế của phương pháp điều khiển trở kháng thụ động trình bày ở mục 7.1.1. Tỉnh 
thần cơ bản của phương pháp điều khiển trở kháng tích cực là thiết kế luật điều 
khiển phản hồi thích hợp cho hệ thống dựa trên các đại lượng đo được là vị trí 
và tốc độ của tay robot cũng như ngoại lực... Hệ thống điều khiển này có thể áp 
dụng cho các robot thực hiện các nhiệm vụ khác nhau bằng thay đổi phần mềm 
của luật điều khiển phản hồi. Trước hết ta xây dựng luật điều khiển cho hợp mô 
hình hệ thống một bậc tự do — môi trường đơn giản, tiếp đó sẽ xây: dựng hệ 
thống điều khiển cho robot có số bậc tự do bất kỳ. 

1. Mô hình hệ thống một bậc tự do đơn giản: Mô hình hệ thống một bậc tự 
do đơn giản trình bày trên hình 7.3a. Phương trình động lực học của hệ thống cơ 
khí có dạng: 

mx+d,X+k,x=fq +f (7-1) 
với: m - khối lượng của vật thể; 

f - ngoại lực tác dụng lên vật thể; 

f„ - lực cần thiết tác dụng lên vật 

thể; 

x - khoảng cách dịch chuyển của 

vật thể so với vị trí cân bằng khi 

lvk Hình 7.3. Mô hình cơ cấu một bậc tự do. 

k, - hệ số lò xo và d, - hệ số giảm chấn. 

Giả thiết trở kháng mong muốn của vật thể đối với ngoại lực tác dụng được 
biểu thị bởi phương trình sau: 

mạX +dạ(X—*¿)+k¿(x—xạ)=Ÿf (2) 
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trong đó: mụ, dụ, kạ - tương ứng là khối lượng, hệ số giảm chấn và hằng số lò xo 
mong muốn; xạ - vị trí đặt. 

Để nhận được hệ thống kín có trở kháng mong muốn (7-2), luật điều khiển 
nhận được bằng thay phương trình (7-2) vào phương trình động lực học (7~l): 

f„ = (m— mạ) +(đ, — dạ)# + (k, —k„)x +da&X¿ +kạXa (7-3) 

Nếu thiết kế m = mụ, luật điều khiển (7-3) sẽ có dạng đơn giản sau: 

f„ =(d, —d¿)& +(k, —k¿)x + dạ#„¿ +k,Xa (7-4) 

Vấn đề thiết kế luật điều khiển là xác định các tham số mụ„ đụ, kạ của 
phương trình luật điều khiển. Xét trường hợp đơn giản khi vật thể không tiếp 
xúc với môi trường, tức là lực f = 0, phương trình (7-2) được viết ở đạng sau: 

mạX+dạX+kạx =d¿X„ +kaX, (7-5 
Biến đổi Laplace phương trình (7-5) và nhận được quan hệ đại lượng dịch 
chuyển thực và đại lượng vị trí thực mô tả bởi hàm truyền: 
ME...) nh... 
(duX¿ +kqxa) p Ÿ ng 
mạ mụ 

Phương trình đặc tính của (7-6) có dạng phương trình kuau dao động với 

tân số dao động œ„ và hệ số suy giảm š được xác định theo các biểu thức sau: 
Ka và d, 
œ,„ = nh, và É= nT, (7-7?) 

Với các chí tiêu tần số dao động tự nhiên và hệ số suy giảm đặt ra, các 
tham số dụ, k„ có thể được xác định theo (7-7) khi gán mạ = m. 

Trường hợp thứ hai, giả thiết vật thể có khối lượng m tiếp xúc với một vật 
thể E cố định như hình 7.4. Tác dụng 
tương tác tại điểm tiếp xúc được mô tả 
bởi phương trình: 

d.xX+k,ŒX —X,)=-f_ (7-8) 


Œ-6) 


hà: 


trong đó: x,„ - vị trí cân bằng khi f = 0. 
Khi bề mặt tiếp xúc của vật thể và E là 
mềm, giá trị k, nhỏ, khi cả hai bể mặt 
cứng, hệ số k, có giá trị lớn. Hệ số d, 
cũng phụ thuộc vào vật liệu hai vật Hình 7.4. Mô hình vật thể tiếp xúc với 
thể. một vật thể cố định. 
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Thay phương trình (7-8) vào phương trình (7-2), nhận được phương trình: 
mạ +(d„ +d,)*+(kạ+k,)x =đạš¿ + kaXx¿+k,xX, (7-9) 
Biến đổi Laplace phương trình (7-9) và nhận được quan hệ tín hiệu đặt và 
tín hiệu ra có đạng tương tự như (7-6): 
X 1/ 
"6n = óa (1-10) 
=....¬ac" 
mạ Hạ 


Tần số dao động và hệ số tắt dần được xác định theo các biểu thức sau: 


œ, " kạ +k, và nh Œ-119) 
mạ 2jm,(k,+k,) 


Nếu các hệ số k„, d, được xác định chính xác, sẽ đễ dàng xác định được mụ, 
dự và kạ thỏa mãn các chỉ tiêu quá trình quá độ mong muốn (thông qua œ, và É). 
Tuy nhiên rất khó xác định các hệ số k, và d,, đặc biệt, nếu giá trị k, lớn hơn giá 
trị tính toán và d, có giá trị nhỏ hơn gía trị tính toán, hệ số suy giảm sẽ giảm và 
đáp ứng quá trình quá độ sẽ tăng với độ qúa điều chỉnh tăng. Vì vậy cần thiết 
phải chọn giá trị dạ khá lớn. Nếu chọn k„, mụ nhỏ, lực tiếp xúc sẽ nhỏ và vật thể 
m sẽ va chạm nhẹ nhàng với vật thể cố định E. Ưu điểm cuả phương pháp điều 
khiển trở kháng tích cực so với điều khiển trở kháng thụ động là có thể điều 
ˆ_ chỉnh trở kháng mong muốn bằng thay đổi giá trị mụ, kạ và d; đảm bảo chất 
lượng quá trình quá độ đặt ra. 





Ví dụ 7.1: Xét hệ thống được mô tả trên hình 7.3 có phương trình động lực 

học dạng sau: 
X+0,lš =fy +Ÿ 

Vật thể m tiếp xúc với một vật thể khác ( hình 7.4) với hệ số d,, k, có giá trị 
sau: 0< d,.< 25 và 600 < k, < 1800 

Cần xác định trở kháng (phương trình 7.2) thoả mãn điều kiện tần số dao 
động tự nhiên trong quá trình tiếp xúc nằm trong khoảng 30 và 60 và hệ số suy 
giảm lớn hơn 0,5. Giá trị m¿ nên chọn gần bằng m và hệ số k„ và d, nên nhỏ. 

Từ phương trình (7.11) ta có: 


30< Ê-ˆ* <60 (7-12) 
mạ 


Sử dụng điều kiện về giá trị của k, ta có: 
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900. m, < K; + 600 (7-13a) 
k, + 1800 < 3600 mụ (7-13b) 
Sử dụng phương trình (7-11) đối với š: 
dạ+d, 
2jmạ(k„+k,) 


Từ đó hệ số d, phải thoả mãn bất đẳng thức: 


>0,5 


d,ạ >./mạ(Œ,+k,)T-đ, (7-14) 
Khi chọn mụ =1, hệ số k„ được xác định từ (7-13): kạ= 500 
Và giá trị nhỏ nhất của d„ xác định từ (7-14): 
đa„¿„ = 2/1.(00 +1800) —15 = 32 
Như vậy trở kháng mong muốn mô tả bởi phương trình: 
X+32(x— X„)+ 500(x — xạ) = f 
Luật điều khiển sẽ có dạng: 
fạ = —32x — 500x +32x„ + 500x,„ 


2. Robot n bậc tự do 

Xét robot n bậc tự do như trên hình 7.5a. Các đại lượng phản hồi là độ dịch 
chuyển § và lực tác dụng lên tay robot. Hệ thống điều khiển sẽ phát các tín 
hiệu điều khiển các hệ truyền động khớp. Ta sẽ chọn luật điều khiển thích hợp 
để tạo ra cho robot các đặc tính động lực học như một robot có cơ cấu cơ khí lý 
tưởng minh hoạ hình 7.5b, đảm bảo tay robot lưôn chuyển động theo quỹ đạo 
đặt trước mà không chịu bất kỳ ngoại lực nào. 

Giả sử cơ cấu cơ khí lý tưởng của tay robot 6 bậc tự do được mô tả bởi 
_ phương trình: ị 

H,¿S+D,§ +K,„s.="°F (7-15) 

trong đó: 5, =S —%5„ - vectơ sai lệch dịch chuyển. Vectơ § biểu thị vị trí và 
định hướng của tay robot; 

7E - ngoại lực tác dụng lên tay robot; 

H,, Đụ, K¿ - ma trận (6x6) tương ứng là ma trận quán tính, ma trận hệ số 
giảm chấn và ma trận độ cứng. Các ma trận này là các ma trận không âm đối 
xứng. Để đơn giản, có thể lựa chọn các ma trận H,, D„, K, là các ma trận đường 
chéo. 


193 





a) b) 
Hình 7.5. Phương pháp điều khiển trở kháng tích cực. 


Động lực học của robot được mô tả bởi phương trình (4-71): 
M=H(4)ä + V(q,q) + G(đ) (7-16) 
Quan hệ giữa vectơ dịch chuyển ỏ toạ độ tay robot § và vectơ biến khớp q 
biểu điễn bởi phương trình động học thuận: 


s=f,() (7-17) 
Vi phân phương trình động học thuận (7-17) nhận được phương trình sau: 
s=1,@)8 (7-18) 
ös 





trong đó: J,(q) = - ma trận Jacobien. 


2q" 
Vectơ mômen của các khớp tương đương với ngoại lực 7# được xác định 

bởi biểu thức (4-113): 
M, =J/(q)'°E (7-19) 


Như vậy phương trình động lực học của robot với tính đến ngoại lực T*F 
tác dụng lên tay robot sẽ có dạng như sau: 


H(q)q + V(,g)+G() = M+M, (7-20) 
Thay (7-19) vào (7-20) ta có: 
H(q)g + V(q,8) + G(g)= M+1}"*E (7-2) 


Giả sử J7 không đơn nhất với mọi q, nhân cả hai vế của (7-21) với J£” ta 
Có: 
J,"H()8 +1;”V(8,3) +1;"G() = J;"M+”*E (22) 
Vị phân phương trình (7-18) ta được: 
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s=],@ä+1,@8 (7-23) 
Rút q từ phương trình (7-23) và thay vào phương trình (7-22), sau một số 
phép biến đổi, nhận được phương trình sau: 


H,(@šS+N,(q,8) =1J;TM+'°F (1-24) 
trong đó: H,(4) = J;”(q)H)JJ,'(4) (7-25a) 

N,@) =J;'(@|V(.3+G(0]-H,()Ï,@Ñ — Œ-25) 
Từ (7-24) nhận được vectơ mômen của khớp có dạng: 

M=1;[H,()s+N,(q,8)-'“F] (1-26) 
Bằng chọn luật điều khiển phản hồi vào-ra có dạng: 

Mụ =1;[H,(g)U+N,(g,8)-"*F] (1-27) 
sẽ nhận được hệ phương trình hệ kín có dạng tuyến tính độc lập: 

§=U (7-28) 
Để nhận được phương trình mong muốn (7-15), tức là: 

S=H/[-(D,š, +K,s,)+”°F] (7-29) 
Chọn: U =§ = H;'[—(D,§, +K,š,)+'"F] (7-30) 


Như vậy luật điều khiển phản hồi có dạng: 
Mụ, =1; ÍN,(4.g)—H,(đ)H7 (@)(D,š, + K,5,)+|H,(4)H;') — 1ƒ“ F} 
(7-31) 
Thay các biểu thức (7-25) vào (7-31) nhận được: 
Mụ„ = V(.Ä) + G()- H(4)!;'J,ä - H(q)1,'H;'(q)(D,Š, + K,8,} 
+[H(1;H;(q)~1]*F 
Nếu chọn H,(q) = H(q) thì luật điều khiển (7-32) có đạng đơn giản hơn 
như sau: 
M, = V(4,8)+G(8)- H1!) ,ä ~ JT|D,5, + K,s, -(K„„ — IƑ'*F| (7-33) 
Khi đó động lực học hệ thống điều khiển kín sẽ mô tả bằng phương trình sau: 
H,š+D,§, +K¿s5, =K„¿'°E (7-34) 


Œ-32) 


Thành phần thứ nhất ở vế phải của luật điều khiển (7-33) bù thành phần lực 
nhớt và lực hướng tâm của robot, thành phần thứ hai bù thành phần trọng lực và 
thành phần thứ ba khử tính phi tuyến của phép biến đổi giữa § và q. Các thành 
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phần thứ nhất và thứ ba có trị số nhỏ khi robot chuyến động ở tốc độ thấp. Khi 
bỏ qua các thành phần đó, thì luật điều khiển (7-33) có đạng sau: 
Mụ„ =G()~J7|D,Š, + K„s, -(K„ —1)E| (1-35) 
Phương trình (7-35) cho thấy rằng luật điều khiển lực theo phương pháp 
điều khiển trở kháng tích cực gồm các thành phần phản hồi theo sai lệch vị trí, 
sai lệch tốc độ và ngoại lực. Với cơ cấu cơ khí cứng: D, = 0 và Ku = l, phương 
trình (7-35) sẽ có dạng đơn giản sau: 
Mụ, =G()~J;K,š, =G()—1KuJ,E, (1-36) 
Luật điều khiển (7-35) được gọi là luật điều khiển độ cứng vì ma trận K, sẽ 
tương ứng với ma trận hệ số độ cứng của cơ cấu. Phương pháp này cũng có thể 
được có tên là điều khiển độ nhún, vì nghịch đảo của ma trận hệ số độ cứng là a 
trận hệ số độ nhún. 
Ví dụ7.2: Thiết kế luật điều khiển trở kháng tích cực cho cơ cấu robot 2 
thanh nối có cấu trúc động học như hình 4.1 
Quan hệ giữa vị trí của tay robot và các biến khớp biểu diễn bởi phương 
trình (4-14) và (4-15): 
=_ L€, +l;€,; 
: "lo #ỉ.Šu | 
trong đó: C¡ kí hiệu cho cosÐ,; S, kí hiệu cho sinÐ;; C¡; kí hiệu cho cos(Ð,+6,); 
S¡; kí hiệu cho sin(0,+9,). 
Ma trận Jacobien J_(đ) được tính như sau: 


đs _ -Ñ§; +l,§¡;) —l;Š¡; _. -l§, ¬l;5¡; 10 
dq 1©) +],Öi; l;C,; l,C, 1C; 1 I 
Vi phân của Jacobien được tính như sau: 


J@:|TieẺ . l ) 


-ISô, ~lI,S;(@,+ỏ,)|t 1 


-] qã= Tạm -L,C„Íô, ni NT 


-Ù§Jð, —1;§;ô, + ô,) 1 119, 
. ˆ ˆ 2 
len I,C„ lô, khi 
IS? 1,S,Í6,+ö,) 
Áp dụng (7-33), luật điều khiển được xác định như sau: 


J,(q)= 
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K;£ hạ;Õ; +2h,„;Õ,6;6 nNG 
5 h„uỡ 5; 


R H; Ỉ lC; l5; 
W %3 . . 3 
„LH H„ |T-C +lC;) ~(8i +l§2)] l,C,Ð: + I;C;Í, +ô„} 
IÌ;§; 1S,ô? +I,S„(ê, +ô, } 
[it +1,§;) L€, +Ì1,C; 
= l;5,; l;C,; 
Sau một số phép biến đổi đơn giản ta viết tách các thành mômen điều khiển 
của các cơ cấu truyền động khớp như sau: 


|b¿, +K,§, -(K„ -1}'*E} 


Mụạy = h,„;Ð; +h,¡;0767 +8; 








: . . 1 

+ LÍH,l,C; -H,(;C; + 1)? +1;[H,i; -H;(C; + I,)|Ê, tô,}} LS 

1292 
+(;S; + IS,)Õ, -{I;C; +1,Cj}*E, 
My; = BuyÊi +8; 

: ì : -. 1 

+ L[H„l,C; -Hz„(;C; +1)? + I,[H,¡]; ~H„(IC; + I,)|ô, tô,}} 1S 
L2 


T6 T6 
+l;§„; F, -l;C; F 


trong đó: '®E_=Dš„„. +K¿s„„ —(K;¿ —1)}f°E 


x x 


TS =Dz§„„ + K,;s„ -(k„¿ ~}E, 


7.2. Điều khiển hỗn hợp 
7.2.1. Ràng buộc tự nhiên và ràng buộc nhân tạo 


Điều khiển tương tác là điều khiển chuyển động của robot theo các lượng 
đặt vị trí và lực trong điều kiện robot robot tiếp xúc với môi trường hoặc nói một 
cách khác là thực hiện điêu khiển chuyển động trong điều kiện ràng buộc với 
môi trường làm việc. 

Phương pháp điều khiển hỗn hợp kết hợp điều khiển lực và điều khiển vị trí 
(quỹ đạo), tuy nhiên lực và vị trí được điều khiển độc lập theo các hướng khác 
nhau. Khi robot làm việc tiếp xúc với môi trường, lực sẽ được điều khiển theo 
._ một hướng, còn điều khiển vị trí được thực hiện theo một hướng khác. 
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Do đặc điểm làm việc của tay robot khi tiếp xúc với môi trường mà chia 
làm hai loại ràng buộc: ràng buộc tự nhiên và ràng buộc nhân tạo. Ràng buộc tự 
nhiên được xác định tự nhiên bởi các nhiệm vụ làm việc của robot trong môi 
trường. Ràng buộc nhân tạo liên quan đến các biến điều khiển được quy định 
bởi chiến lược điều khiển robot. Để thuận lợi cho việc mô tả nhiệm vụ robot 
thực hiện và việc xác định các ràng buộc tự nhiên và nhân tạo, sẽ sử dụng khung 
toạ độ ràng buộc (0, x, y, z„ ) không trùng với khung toạ độ gốc robot. Quan hệ 
giứa khung toạ độ ràng buộc và khung toạ độ gốc được biểu thị bằng ma trận 
biến đổi quay °R, biểu diễn khung toạ độ ràng buộc so với khung toạ độ và °Rạ 
biểu diễn khung toạ độ gốc so với khung toạ độ ràng buộc. 

Ví du7.3: Một số trường hợp. cơ cấu robot làm việc với môi trường điển 
hình: 

1, Cơ cấu tay robot gắp một vật và đi chuyển một vật di chuyển trên bể 
mặt phẳng (hình 7.6). Khung toạ độ ràng buộc được gắn trên bể mặt tiếp xúc 
giữa vật thể và mặt trượt. Tay robot chỉ di chuyển tịnh tiến vật thể đọc theo trục 
x, và y, nên không có dịch chuyển tịnh tiến theo trục z„ đồng thời không có 
chuyển động quay theo các trục x, và y,. Nếu bỏ qua lực ma sát ở mặt trượt, sẽ 
không có lực tác đụng dọc theo hướng x, và y„ và không có mômen quay theo 
trục z„. Các điều kiện đó là các ràng buộc tự nhiên. Các ràng buộc nhân tạo liên 
quan đến các biến không phải là các ràng buộc tự nhiên: Tốc độ di chuyển tịnh 
tiến theo các trục x, và y, và tốc độ quay xung quanh trục z„; Lực tác dụng dọc 
theo các trục X, và y, và mômen quay của trục z„. Các ràng buộc tự nhiên và 
nhân tạo được minh họa trong bảng 7.1. 

2. Cơ cấu tay lắp một chỉ tiết vào một lỗ hình trụ (hình 7.7). Khung toạ độ 


Bảng 7.1. 
Ràng buộc tự nhiên | Ràng buộc nhân tạo 





sự, °Ec 





a) b) 
Hình 7,8, Cơ cấu tay di chuyển tịnh tiến vật thể và các rằng buộc tự nhiên và nhân tạo: 
a- cơ cấu tay di chuyển tịnh tiến vật thể; b- ràng buộc tự nhiên và nhân tạo. 
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ràng buộc được chọn có trục z„ trùng với thực của chỉ tiết. Ràng buộc tự nhiên 
được xác định từ điều kiện lắp ráp: không có chuyển động tịnh tiến và quay theo 
các trục x, và y„ đồng thời không.có lực tác dụng dọc theo trục z, và mômen 
quay xung quanh trục z„. Như vậy, các điều kiện ràng buộc nhân tạo là các biến 
còn lại: Lực và mômen tác dụng theo các trục x, và y,; tốc độ dịch chuyển tịnh 
tiến và tốc độ góc theo trục z„. Bảng 7.2 mô tả các ràng buộc tự nhiên và nhân 
tạo của cơ cấu. 


Bảng 7.2. 


Ràng buộc tự nhiên | Ràng buộc nhân tạo 





a) b) 
Hình 7.7. Cơ cấu tay lắp ráp chỉ tiết vào một lỗ và các ràng buộc tự nhiên và nhân tạo: 
a- cơ cấu tay lắp ráp chỉ tiết vào một lỗ; b- ràng buộc tự nhiên và nhân tạo. 


3. Cơ cấu quay một cánh tay đòn (hình 7.8). Khung toạ độ ràng buộc đặt ở 
đâu mút của cánh tay đòn của cơ cấu quay. Ràng buộc tự nhiên được xác định 
bởi điều kiện làm việc tự nhiên của cơ cấu là không có di chuyển đọc theo trục 
z, và tốc độ góc xung quanh trục x, và y„ đồng thời không có lực tác dụng theo 
trục y„ và mômen quay theo trục z„. Các ràng buộc nhân tạo là tốc độ dịch 
chuyển tịnh tiến theo trục y,; tốc độ quay xung quanh trục Z„; các lực tác dụng 
theo các trục x, và z. và mômen quay theo các trục x, và y,. Bảng 7.3 mô tả các 
ràng buộc tự nhiên và nhân tạo của cơ cấu. 

Bảng 7.3. 


Ràng buộc tự nhiên | Ràng buộc nhân tạo 





a) b) 


Hình 7.8. Cơ cấu tay quay một cánh tay đòn và các ràng buộc tự nhiên và nhân tạo: 
a- cơ cấu tay quay một cánh tay đòn; b- ràng buộc tự nhiên và nhân tạo. 
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7.2.2. Điều khiển hỗn hợp trên cơ sở mạch phản hồi kinh điển 

Dựa trên việc mô tả các biến ràng buộc tự nhiên và nhân tạo, hệ thống điêu 
khiển hỗn hợp chỉ thực hiện điều khiển các biến ràng buộc nhân tạo mà không 
điêu khiển các biến ràng buộc tự nhiên để đảm bảo tránh các sự xung đột giữa 
điều khiển và môi trường làm việc. 

Các biến ràng buộc tự nhiên được mô tả bởi một tập các biến tốc độ và lực 
biểu điễn trong khung toạ độ ràng buộc và được biểu điễn ở dạng: 

>°v=°v„ và (1—>)°F=°F,„ (7-37) 


e 


trong đó: °V,„, °EF_ - tương ứng là các thành phần tốc độ và lực mô tả trong 
khung toạ độ ràng buộc. 

Với -các biến ràng buộc tự nhiên được định nghĩa theo (7-37), các biến ràng 
buộc nhân tạo được mô tả bởi một tập các biến tốc độ và lực biểu diễn trong 
khung toạ độ ràng buộc và được biểu diễn ở dạng: 

(I—5)'V=°V„ và X°F='F, Œ-38) 
trong đó: °V„, °E„ - tương ứng là các thành phần tốc độ và lực mô tả trong 
khung toa độ ràng buộc. Ma trận 3, là ma trận lựa chọn cho phép lựa chọn các 
biến điều khiến thích hợp cho mỗi bậc tự do của robot. 

Sơ đồ khối tổng quát của hệ thống điều khiển hợp lực - vị trí được mô tả 
trên hình 7.9 với giả thiết tất cả các biến đầu ra cần thiết cho điều khiển lực - vị 
trí có thể đo được. 

Đối với hệ thống điều khiển vị trí, tín hiệu phản hồi trong trường hợp đơn 
giản có thể là sai lệch vị trí Š và tốc độ §; lượng đặt là tốc độ và gia tốc; các 
biến ràng buộc nhân tạo được định nghĩa theo điều kiện làm việc của cơ cấu. 
Tương tự, đối với hệ thống điều khiển lực, tín hiệu phản hồi là lực tiếp xúc °F; 
lượng đầu vào là lực đặt và các biến ràng buộc nhân tạo được định nghĩa theo 
điều kiện làm việc của cơ cấu. 

Các tín hiệu phản hồi sai lệch vị trí 5 , tốc độ s và lực tiếp xúc °E được 
biến đổi thành các đại lượng đồng nhất tương ứng với các đại lượng ràng buộc 
nhờ phép biến đổi T„. Các đại lượng nhận được được biểu diễn trong khung toạ 
độ gốc. Sau đó các đại lượng đó tiếp tục được biến đổi về khung toạ độ ràng 
buộc bằng nhân phía trước với ma trận quay °Rạ. 

Trên cơ sở ma trận lựa chọn được xác định, sẽ tính được các tín hiệu phản 
hồi cần thiết cho điều khiển vị trí và lực tương ứng với các tín hiệu chủ đạo đâu 
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vào. Tín hiệu điều khiển tổng đầu ra bộ điều khiển °F, gồm hai thành phần 
°F¿„ và °F¿; được biến đổi về khung toạ độ gốc bằng phép biến đổi °R,. 





Hình 7.9. Sơ đồ khối tổng quát của hệ thống điều khiển hỗn hợp. 


Cuối cùng, tùy thuộc vào hệ thống điều khiển thực tế, có thể bổ sung một 
khâu bù, hiệu chỉnh phi tuyến cho cả lực và vị trí. Đầu ra là mômen đầu vào của 
các khớp robot. 

Ví dụ 7.4: Xét một robot 2 thanh nối có tay di chuyển tựa trên bề mặt của 
một đối tượng như hình 7.10. Gắn một hệ trục toa độ (O,x,y,) vào đối tượng như 
chỉ trên hình vẽ. Tay robot di chuyển theo hướng O,x, và sình ra lực theo hướng 
O,y,. Như vậy vị trí tay robot được điều khiển theo hướng O,x, và lực được điều 
khiển theo hướng O,y,. Vị trí và lực của tay robot trong hệ toạ độ O,xy„ được 
đo bằng các cảm biến vị trí và lực đặt ở tay robot. 

Với điều kiện làm việc của cơ cấu hình 7.10, ma trận lựa chọn có dạng: 


"nh ¡ 


Sai số vị trí và lực của tay robot trong hệ toạ độ O,x,y, được xác định theo 
phương trình ma trận sau: 


°§ (0= Đ{°s,(@-°s(9| (7-39) 

°E(@=šz|°E,@-°EF@] (1-40) 

trong đó S„(t), “S(f) - vectơ vị trí đặt và vị trí thực của tay robot trong hệ toa 
độ O,X.ÿ.; 
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“EF,(), “F() - vectơ lực đặt và lực sinh ra ở tay robot trong hệ toạ độ 
O.X.V,. 
Vectơ sai số vị trí và lực ở tay Tc 
robot trong hệ toạ độ O,x.y, được 
biến đổi về hệ toạ độ khớp theo các 
biểu thức sau: 
q,()=17'°5 (Ð (7-41) 
q,(=12'°§() — Œ42) 
M.(Œ)=1JƑ°F() (43 
trong đó J, là ma trận Jacobien. 
Các mômen điều khiển của các 
khớp là tổng của hai thành phần 





X 
Hình 7.10. Cơ cấu robot với điều khiển hỗn hợp. 


Mạy Và Mu, - Thành phân mômen Mí„. là đầu ra của bộ điều khiến vị trí sẽ 
bù sai lệch vị trí và Mặy là đâu ra của bộ điều khiển lực sẽ bù thành phần sai 
lệch lực. Có thể lựa chọn bộ điều khiển vị trí và bộ điều khiển lực có cấu trúc 


khác nhau. Thông thường bộ điều khiển có cấu trúc PI, bộ điều khiển lực có cấu 
trúc L. Khi đó luật điều khiển vị trí và lực có dạng sau: 


My ()=K;q,@)+K;q,@) (7-44) 
Mặz(Ð =K; ịM. (t)ẻt (1-45) 
0 


Mômen tổng điều khiển khớp là: 
Mă(Ð =May()+M„c() (7-46) 

Các phương trình (7-39)-(7-46) là các thuật toán điều khiến hỗn hợp của 
robot và sơ đồ khối của hệ thống điều khiển hỗn hợp robot. Một số tác giả [15] 
đã để xuất luật điều khiển vị trí là PID và luật điều khiển lực kết hợp cấu trúc PI 
với thành phần tiền định tỷ lệ với tín hiệu lực đặt. 

7.2.3. Điều khiển hỗn hợp trên cơ sở cấu trúc động lực học robot 

Bộ điều khiển hỗn hợp được xây dựng gồm hai phần: Bộ điều khiển tuyến _ 
tính hóa phản hồi nhằm khử tính phi tuyến và ràng buộc của hệ thống và nhận 
được hệ thống độc lập và tuyến tính; Bộ điều khiển thứ hai được thiết kế để cơ 
cấu tay robot bám theo quỹ đạo mong muốn và sinh lực cần thiết. 
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Quan hệ giữa vectơ dịch chuyển ở toạ độ tay robot § và vectơ biến khớp q 
biểu diễn bởi phương trình động học thuận: 


s=f.) Œ-4?) 
Đạo hàm cấp hai phương trình (7-47) nhận được phương trình sau: 
`s=1()8 +1)Ä (1-48) 


trong đó: J(đ) - ma trận Jacobien. Giải phương trình (7-48) nhận được vectơ gia 
tốc của khớp: 
q=1"4\Iš - j4] (1-49) 

Thay (7-49) vào phương trình động lực học của robot (4-73) với kể đến 
thành phần lực ma sát trong bộ truyền và thành phần mômen khớp gây ra bởi 
ngoại lực tác dụng lên tay robot; 

M=H()1 `(@JŠ - Ï(qJj]+ C(4,4)4 + G()+F(4)+J”'“F (7-50) 

Bộ điều khiến tuyến tính phản hồi được lựa chọn như sau: 

Mụ„ = H(4)J”'(q)[U - J(q)đ]+ C(,đ)8 + G()+F()+J"”*E (7-51) 

Khi đó hệ thống kín sẽ được mô tả bởi một hệ phương trình tuyến tính và 
độc lập: : 

s=ÙU (1-52) 

với U- vectơ (nx1) biểu thị luật điều khiển tuyến tính quỹ đạo và lực của tay 
robot. Phương trình (7-52) cho thấy rằng chuyển động của tay robot được tách 
rời độc lập và tuyến tính. Do đó có thể thiết kế các bộ điều khiển vị trí và lực 
độc lập nhau: Bộ điều khiển vị trí được thiết kế đảm bảo tay robot chuyển động 
theo phương tiếp tuyến với quỹ đạo chuyển động và bộ điều khiển lực thiết kế 
đảm bảo tay robot sinh lực theo hướng trực giao với hướng chuyển động. 

Phương trình (7-52) được viết tách thành các phương trình mô tả chuyển 
động theo phương tiếp tuyến và trực giao trong không gian tay robot. Nếu ký 
hiệu sy, là thành phần tiếp tuyến thứ ¡ của S. Phương trình mô tả chuyển động 
tiếp tuyến của tay robot được viết ở dạng: 
Š„ = tị, (7-53) 
trong đó: ur, - tín hiệu điều khiển vị trí theo phương tiếp tuyến của thành phần 
thứ ¡. 

Bộ điều khiển vị trí theo phương tiếp tuyến cho thành phần thứ ¡ tương ứng 
có thể lựa chọn theo cấu trúc PD ở đạng: 


1; = Srạ¡ + k.yy Sự, + kipj Š, Œ-54) 
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vỚI §+;, Krụ, kạu tương ứng là gia tốc đặt cho thành phần thứ ¡, hệ số tỷ lệ đạo 
hàm và hệ số tỷ lệ của thành phần thứ ¡; 
ấ,,, 5„ tương ứng là sai lệch vị trí và sai lệch tốc độ của thành phần tiếp 
tuyến thứ ¡: 
Šn “Sa —Sm Œ-55a) 
Nà =ŠTdi —Š1¡ (7-55b) 
Kết hợp (7-53) và (7-54), phương trình mô tả sai lệch của hệ thống kín 
được viết ở đạng sau: 


~ 


Srại + Kia Šm + kru Š =0 Œ-56) 
Các hệ số krạ và k,; được tính toán sao cho hệ thống ổn định tiệm cận và 
sai lệch hội tụ về không. 
Tương tự, ký hiệu sụ; là thành phần trực giao thứ j của S. 
Phương trình mô tả chuyển động trực giao của tay robot được viết ở dạng: 

ẩm = Un Œ-57) 
trong đó: uạ; - tín hiệu điều khiển lực theo phương trực giao của thành phần thứ 
1. Giả sử môi trường làm việc của robot có dạng đàn hồi, lực trực giao tác dụng 
vào môi trường được tính theo công thức sau: 

ENy = KoÁSNj — Saj) ñ (7-58) 
với k„ là thành phần j của độ cứng của mô trường, s„ là vị trí tĩnh của môi 
trường theo hướng trực giao Sạ;. 

Đạo hàm phương trình (7-58) nhận được: 


\ le 
Su “—— Eụ (7-59) 
kụ 
Kết hợp (7-57) và (7-59) ta có: 
TNG 
¡ Ry =Ủu (7-60) 


sỉ 
Bộ điều khiển lực tương ứng có thể lựa chọn có cấu trúc PD ở đạng: 
1 + _y 
VN, = b6 CỀNg + Kgqsfy + ky» Ey) Œ-61) 
$j 
trong đó kuu; và ku„ tương ứng là các hệ số đạo hàm và hệ số tỷ lệ. Các hệ số 
này sẽ được chọn đảm bảo hệ ổn định tiệm cận và sai lệch lực tiến tới không. 
FN , E„ tương ứng là sai lệch lực và sai lệch đạo hàm lực của thành phân 
trực giao thứ j: 
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Eặ = ENdj = Eụy (7-61a) 

Tụ = Ẩg — Êu Œ-61b) 

Để đo lực tác dụng lên tay robot, có thể sử dụng cảm biến lực gắn trên 

cổ tay robot. Tuy nhiên vectơ lực đo được bằng cảm biến lực ở cổ tay không 

phải là lực tác dụng lên tay robot. Vì vậy cân phải biến đổi vectơ lực từ cảm 
biến lực về vectơ lực ở tay robot theo công thức sau: 

J'()F=1 œ (Fks (7-62) 
trong đó: F, Rạp tương ứng là vectơ lực trong không gian làm việc và vectơ lực 
đo được bằng cảm biến lực; 

J(q) ;Jc; (q) tương ứng là ma trận Jacobien của không gian tay và ma 
trận Jacobien của cảm biến. 
Từ (7-62), vectơ lực tác dụng lên tay robot được xác định theo công thức 
SaU: 
F=1”(1a(q)E.; (7-63) 
Thành phần đạo hàm lực cần thiết cho tính sai lệch lực ở phương trình 
(7-61) được bằng sử dụng (7-58): 
Fặ = Kiên Œ-6% 
Sơ đồ khối hệ thống điều khiển hỗn hợp dựa trên cấu trúc động lực học 
robot được trình bày trên hình 7. 1. 










ENg „ FNg; 






Đn; 


Luật ĐK lực 


Lựa chọn không 
gian trực giao 


Hình 7.11. Sơ đổ khối của hệ thống điều khiển hỗn hợp dựa trên mö hình động lực học, 
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Ví dụ 7.5: Thiết kế hệ thống điều khiển hỗn hợp vị trí/1ực cho một cơ cấu 
tay robot 2 bậc tự do trong mặt phẳng như hình 7.12. Mục đích điều khiển đảm 
bảo tay robot di chuyển dọc theo hướng x với quỹ đạo xr„ = 0,2 + 0,05sin(t) m 
và sinh lực theo hướng y với lực Fyya = l = e. Hệ số độ cứng k, = 1000 N/m. VỊ 

- trí ban đầu của tay robot là: x(0) = 0,25 m và y(0) = y, = 0m. 
Các tham số robot: Khối lượng thanh nối: mạ = 2,5 kg; m; = 1,5 kg. 
Khối lượng tải: m, = 0,5 kg. 
Chiểu dài thanh nối: a = 0,25 m; a, = 0,15 m. 





M, 





Hình 7.12, Cơ cấu robot 2 bậc tự do. 
Giả thiết bỏ qua lực ma sát ở các khớp, lực trực giao phụ thuộc vào vị trí 
theo quan hệ: 
Fy = kí(y — y,) 
Sử dụng (7-5 L) luật điều khiển hỗn hợp được thiết kế ở dạng: 
Mụ, = H4)!” (q}U + C(,q)4 + G(4) + J7 ”*F 
trong đó: U (2x1) - vectơ tín hiệu điều khiển gồm hai thành phần: điều khiển vị 
trí và lực; 
H, C, G là các ma trận của phương trình động lực học robot 2 thanh nối (4-29); 
T6 Ƒ - ngoại lực tác dụng vào tay robot. 
Với bộ điều khiển lựa chọn như trên, hệ thống kín của hệ thống sẽ được mô 
tả bởi hệ phương trình độc lập tuyến tính: 


Nó kg 
Hy 


Thành phần tiếp tuyến là dịch chuyển của tay và thành phần trực giao là lực 
sinh ra ở tay trong quá trình đi chuyển. Bộ điều khiển thành phần tiếp tuyến — di 
chuyển của tay được thiết kế theo (7-54): 


uy =*„£a4 +k,pấy tk, ấy 
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Và bộ điều khiển thành phần trực giao - lực của tay được thiết kế theo (7-61): 


Wyẻ xa +kpÑy +kyẼ, ) 
trong đó: xr, =0,2+ 0,05sin(Ð) m; Fy¿ = 1 — e*và k, = 1000 N/m. 
Các hệ số kp (k,p và km) và kẹ (k„; và k„) của luật điều khiển có thể xác 
định theo điều kiện ổn định và hội tụ: 
ko=2ŠÉ@, và kẹp = œ,? 
với Š, œ là hệ số suy giảm và tần số dao động của hệ. Các hệ số này được lựa 
chọn theo chỉ tiêu quá trình quá độ yêu cầu: Độ quá điều chỉnh œ% và thời gian 
quá trình quá độ tua. 
Giả sử yêu cầu độ quá điều chỉnh nhỏ hơn 20% và thời gian quá trình quá 
độ là 1,5 s. Chọn š = 0,7, Tần số dao động được xác định theo công thức: 


Do đó: ko = 2œ, = 2.0,7.3,8 = 5,32 và kẹ= œ„?= 3,82 = 14,44. 
Kết quả mô phỏng được trình bày trên hình 7. 13. 
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Hình 7.13. Kết quả mô phỏng của ví dụ 7.5. 
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Phụ lục 
Chương trình mô phỏng MATLAB cho ví dụ 7.5 


1. File chương trình mô phỏng 

%Chuong trinh mo phong he thong dieu khien luc robot 2 thanh noi 
% Controller2: Bo đieu khien luc 

%x11=x0(1);%Goc thetaT 

%x12=x0(2),%Toc do Goc khop I 

%x21=x0(3);%Goc theta2 

%x22=x0(4);%Toc do Goc khop 2 


S%œ(0.25,0]; 

theta=vitridau(S0); 

Tk=0.001; 

x0-=[theta(1);0;theta(2);0]; 

file1=fopen(datal.$','w`; 

for t=0:Tk:10; 

sd=%tpoint(t); 
[M,xe,fe]=Controller1(sd,S0(2),x0(1),x0),x0(2),x0(4)): 
[s,X]=Robot2(M,x0,TK); 

x0=x:; 

fprintf(file !,'%2,.4f %2.4f %2.4f %2.4f %3.4f %3.4fn'.t,sđ(1),s(1),fe,M(L),M(2)); 
end 

fclose(file 1); 


2. Hàm tính luật điều khiển 
function {M,xe,fe]=Controller I{sd,yo,theta I ,theta2,dthefa l ,dtheta2) 
%Bo dieu khien luc hon hop 








%Cac thong so cua Robot 
m1=2.5;%Khoi luong thanh I 
m2=1.5;%Khoi luong thanh 2 
mt=0.5; %Khoi luong tai 

a1 =0.25;%Chieu đai thanh 1 
a2 =0.15; %Chieu dai thanh 2 
xl I=thetal; 

x12=dthctal; 

x2l=theta2; 

x22=dtheta2; 

Cl=cos(xI L); 

C2=cos(x2l); 


Cl2=cos(xI I+x21); 
Sl=sin(x11!); 


§2=sin(x2l); 
§12=sin(x11+x2l}; 
đan si 2A 22x sc, 


H1I=(nt+m2)*al^2+m2*a2^2+2*m2*a1*a2*C2; 
H12=m2*a2^2+m2*a1*a2*C2; 
H22=m2*a2^2; 

H21=HI12; 

H={H11,H12;H21,H22]; 

S12 2z 
v]=-m2*al*a2*s2*x22^2-2*m2*a1*a2*s2*x12*x22; 
v2=m2*al*a2*s2*xI2^2; 

V-=[vI;v2}; 

c2 te zcae : 
G1=(m]+m2)*9.81*a1*C1+m2*9.81*a2*C12; 
G2=m2*9.8]*a2*C12; 

G-={GI;G2]; 

Ø~~~>-~-xe---~==== 

%Ma tran Jacobien 
J=[-al*s1-a2*s12,-a2*s12;a1*C1+a2*C12,a2*C12}; 
%Tinh vi tri va toc đo tay 
s=[al*CI+a2*C12;a1*sI+a2*s12]; 
đs=J*{x12;x22]; 

#%Tinh luc va dao ham luc 

f=ke*(s(2)-yo); 

df=ke*ds(2); 

%Sai lech vi tri tay va sai lech luc tay 
xe=sđ(1)-s(1}; 

dxe=sd(3)-ds(L); 

fe=sd(2)-f, 

dfe=sd(4)-df; 

%Luat dieu khien hơn họp 
kdx=5.32;kdf=kdx; 

kpx=14.44,kpf=kpx; 
Udk1=sd(5)+kdx*dxe+kpx*xe; 
Udk2=(sd(6)+kdf*dfe+kpf*fe)/ke; 
Udk=[Udk1;Udk2]; 

M=H*inv(Đ*Udk+G+V; 


3. Hàm tính lượng đặt 


function Sd=Setpoint(u) 

% Lương dat qui đao va lục o tay 
t=u(1);Xdm=0.02;:Xd0=0.2; 
xd=Xdm*%sin(t)+Xd0; 
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fd=l-exp(-1); 
dxd=Xdm*cos(); 

đfd=exp(-1); 
đdxd=-Xdm*sin(t); 
đdfd=-exp(-t); 
Sd=[xd;fd;dxd;dfd;ddxd;ddfd]; 


3. Hàm tính mô hình Robot 


function fs,X]=Robot2(M,X0,Tk) 

%Mo phong robot 2 thanh noi 
Mol=M(1);%Mo men dieu khien khopl 
Mo2=M(2);%Mo men dieu khien khop2 
xII=X0(1);%Goc thetal 
xI2=X0(2);%Toc do Goc khop ] 
x21l=X0(3);%Goec theta2 
x22=X0(4);ZToc do Goọc khop 2 


%Cac thong so cua Robot 
m1=2.5;%Khoi luong thanh 1 
m2=1.5;%Khoi luong thanh 2 
mt=0.5; %Khoi luong tai 

al =0,25;%Chieu dai thanh I 
a2 =0.15; %Chieu đai thanh 2 


%===—=—=====~z~=~E——=~~===================~>====—— 


Cl=cos(xI L); 

=cos(x2 L}; 
C12=cos(xLI+x2l); 
S1=sin(x1 1); 
S2=sin(x2L); 
S12=sin(xI1+x2l); 
%Ma tran quan tỉnh 
HII1=(ml+m2)*al^2+m2*a2^2+2*m2*al*a2*C2; 
HI2=m2*a2^2+m2*a1*a2*C2; 
H22=m2*a2^2; 
H2I=HI2; 
H=[H11,HI2;H21,H22)}; 
Ø%Mo men nhot va huong tam 
v(1)=-m2*al*a2*S2*x22^2-2*m2*al*a2*S2*xI2*x22; 
v(2)=m2*al*a2*S2*xI2^2;, 
V=[v(1);v(2)]; 
%Mo men trong luc 
GI=ứn1+m2)*9.81*a1*CI+m2*9.81*a2*C12; 
G2=m2*9,8I*a2*C12; 
G={G1;G2]; 
%ïNgich dao ma tran H 


Hinv=inv(H); 

#%Gta toc khop 
dX=-Hinv*(V+G)+Hinv#*[Mol;Mo2]; 
%Phuong trinh trang thai 

xIIp=xl2; 

x2lp=x22; 

x12p-dX(I);, 

x22p=dX(@2), 

%Tinh gan dung phuong trinh vi phan 
x1I=x†1+Tk*xIIp; 
x2I=x2l+Tk*x2Ip; 
XI2=x12+Tk*x12p, 
X22=x22+Tk*x22n; 
X=[xI1;x12;x21;x22]; 

% "Tinh vi trí tay robot 

Cl=cos(x1l); 

C12=cos(x1 I+x2l1); 

Sl=sin(x1 1); 

S12=sin(x11+x2l); 
s=[a1*C1+a2*C12;aL*S1+a2*S12]; 
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CHƯƠNG 8 
CƠ CẤU CHẤP HÀNH VÀ 
CẤU TRÚC HỆ THỐNG ĐIỀU KHIỂN 


Cơ cấu chấp hành là “cơ bắp” của robot, hệ thống thanh nối và khớp là “bộ 
xương” của robot. Cơ cấu chấp hành là cơ bắp sẽ có chức năng là chuyển động 
(quay, tịnh tiến) các thanh nối dẫn đến thay đổi cấu hình của robot. Các dạng cơ 
cấu chấp hành là: Động cơ điện gồm động cơ bước, động cơ secvo, động cơ 
truyền động trực tiếp; cơ cấu chấp hành thủy lực; cơ cấu chấp hành khí nén. 
Trong chương 8 sẽ mô tả các đặc tính và nguyên lý vận hành của các cơ cấu 
chấp hành sử dụng trong robot. Phần cuối chương sẽ trình bày cấu trúc của hệ 
thống điều khiển cơ cấu chấp hành. 


8.1. Các đặc tính của hệ thống cơ cấu chấp hành 

8.1.1. Trọng lượng và tỷ số công suất - trọng lượng 

Trọng lượng (W) và tỷ số công suất - trọng lượng (P/W) là những tham số 
quan trọng của hệ thống cơ cấu chấp hành. Hệ thống truyền động điện có tỷ số 
P/W cỡ trung bình. Động cơ bước thường trọng lượng lớn hơn động cơ secvo có 
cùng công suất nên có tỷ số P/W thấp hơn. Điện áp động cơ càng cao, tỷ số P/W 
càng cao. 

Hệ thống khí nén có tỷ số P/W nhỏ nhất; trong khi đó hệ thống thủy lực có 
tỷ số P/W lớn nhất. Thực tế, hệ thống thủy lực gồm hai phân: Cơ cấu chấp hành 
thủy lực và bộ phân cung cấp năng lượng gồm bơm, bộ phận làm lạnh, van... 
Các thiết bị này có trọng lượng lớn, nhưng là thiết bị nh và thường được đặt ở 
một nơi cố định không đặt ở trên cơ cấu chuyển động robot và không tính vào 
trọng lượng của các cơ cấu chấp hành của robot. Do đó, trọng lượng cơ cấu chấp 
hành của hệ thống truyền động thủy lực là nhỏ nhất tương ứng là tỷ số P/W lớn 
nhất. 
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8.1.2. Độ cứng - độ nhún 


Độ cứng là phản ứng của vật liệu chống lại sự biến dạng. Độ cứng càng 
cao, lực làm biến dạng vật liệu càng lớn hoặc độ biến dạng của vật liệu càng 
nhỏ. Ngược lại, vật liệu có độ nhún lớn tức là đễ đàng bị biến đạng khi có lực 
tác dụng. Độ cứng được biểu thị thông qua đại lượng môđun đàn hồi. Cơ cấu 
thủy lực có môđun đàn hồi cao, tương ứng sẽ có độ nhún thấp. Ngược lại, cơ 
cấu khí nén có độ nhún cao. 

Cơ cấu có độ cứng cao sẽ phản ứng nhanh với sự thay đổi của lực và sẽ đạt 
độ chính xác chuyển động cao. Cơ cấu có độ nhún cao sẽ dễ bị biến dạng, nên 
có độ chính xác thấp. Như vậy, cơ cấu thủy lực có độ chính xác vị trí cao hơn cơ 
cấu khí nén. Ví dụ xét một robot lắp một chip vi mạch nhiều chân vào bảng 
mạch in, nếu độ cứng thấp, sự lắp ráp sẽ khó khăn vì cơ cấu có thể bị biến dạng 
dưới tác dụng của phản lực. Ngược lại, khi lắp một chỉ tiết vào một lỗ với độ kín 
khít cao, cơ cấu có độ cứng cao sẽ khó lắp ráp và có thể gây ra hỏng chỉ tiết. 


8.1.3. Bộ truyền giảm tốc 


Cơ cấu thủy lực có khả năng sinh lực rất lớn với xung lực ngắn. Do đó cơ 
cấu thủy lực có khả năng di chuyển nhẹ nhàng, êm mà không cần sinh lực lớn. 
Vì lý do đó, không cần khâu giảm tốc trọng hệ thống truyền động thủy lực. Cơ 
cấu thủy lực thông thường được nối trực tiếp với thanh nối, làm đơn giản hệ 
thống, giảm trọng lượng, giá thành thấp, mômen quán tính nhỏ, giảm khe hở và 
tăng tính khả thi chế tạo do cơ cấu gồm ít các phần tử và không gây ồn. 

Động cơ điện thường có tốc độ cao, cần hộp truyền trong hệ thống truyền 
động để tăng mômen khớp và giảm tốc độ chuyển động thanh nối. Vì VẬY sẽ 
làm tăng gía thành, tăng khe hở, mômen quần tính... 

Xét một sơ đồ động học của cơ cấu truyền động sử dụng động cơ điện như 
hình 8.1. Mômen và tỷ lệ tốc độ giữa động cơ và khớp được biểu thị bằng công 
thức sau: 

My =iMp 
Ũ ốp (8-1) 
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Hình 8.1. Cơ cấu truyền động của khớp robot. 


Phương trình cân bằng mômen của hệ thống có dạng: 


(rồy + BrÕ+) 


: ;  M : : 
Mp =lpôp + Bpốp +—_T =Jpỗp + BgỐp + 
1 l 


(lxÖp T Brôp) 
:2 
Ì 


l2. B... 
=Qp + 3Ểo +(Bp +¬g)Ö 


= JpỐp + BpÖp + (8-2) 


trong đó: 

Mạ, Mỹ - tương ứng là mômen khớp và mômen động cơ; 

1¿, lp - tương ứng là mômen quán tính khớp và mômen quán tính động cơ; 
1 - tỷ số truyền của hộp giảm tốc. 

B¡, Bọ - tương ứng là hệ số ma sát nhớt khớp và hệ số ma sát nhớt động cơ; 
Ô;, Ôp - tương ứng là góc quay khớp và góc quay động cơ; 

Ô„,Ô,,Ôr,Ôp- tương ứng tốc độ góc khớp và tốc độ góc động cơ, gia tốc 

góc khớp và gia tốc góc động cơ. 

Mômen quán tính tổng quy đổi về trục động cơ được tính từ (8-2): 


J 
J; =Œp ta) (8-3) 


Thông thường tỷ số truyền hộp giảm tốc lớn, nên mômen quán tính khớp ít 
ảnh hưởng đến quán tính của hệ thống. Do đó động cơ có khả năng gia tốc rất 
nhanh, đặc biệt ở hệ thống có tỷ số truyền lớn. 


214 


8.1.4. Tóm tắt các đặc điềm cúa các hệ thống cơ cấu chấp hành 


Bảng 8.1. Tóm tắt các đặc điểm của các hệ thống 


+ Sử dụng tốt cho robot |+ Sử dụng cho mọi cỡ|+ Nhiều phần tử đặt cố 


cỡ lớn và tải trọng lớn 

+ Tỷ số P/W cao nhất. 

+ Độ cứng cao, độ 
chính xác cao, phản ứng 
nhanh. 

+ Không cần hộp giảm 
tốc. 

+ Có thể làm việc với 
đải tốc độ rộng. 

+ Không dễ bị hư hỏng. 
+ Do có rò rỉ nên không 
thích hợp với phòng làm 
việc sạch sẽ. 

+ Cần có thiết bị: bơm, 
động cơ, van. 

+ Tiếng ồn, yêu cầu bảo 
dưỡng cao. 

+ Độ nhớt dầu thay đổi 
theo nhiệt độ. 

+ Mômen, áp lực, 
mômen quán tính của 
cơ cấu lớn. 


TOboOI, 

+ Chất lượng điều khiển 
tốt, độ chính xác cao. 

+ Độ nhún cao hơn hệ 
thủy lực. 

+ Ảnh hưởng của mômen 
quán tính tải bé do có bộ 
truyền. 

+ Thích hợp làm việc 
trong các phòng sạch sẽ 
do không có rò rỉ. 

+ Độ tin cậy cao, nhu cầu 
bảo dưỡng nhỏ. 

+ Hệ thống truyền động 
cần hộp giảm tốc. 

+ Động cơ cần thiết bị 
hãm khi không cấp điện 


8.2. Cơ cấu chấp hành thủy lực 
8.2.1. Nguyên lý cơ bản của cơ cấu chấp hành thủy lực 


định. 

+ Các phần tử có độ tin 
cậy cao. 

+ Cơ cấu nhún cao. 

+ Không rò rỉ. 

+ Rẻ và đơn giản. 

+ Áp suất nhỏ hơn so 
với cơ cấu thủy lực. 

+ Tiếng ồn lớn. 

+ Cần các thiết bị: máy 
nén, bộ lọc. 

+ Khó điều khiển vị trí 
tuyến tính.. 

+ Độ cưng rất nhỏ, phản 
ứng không chính xác. 

+ Tỷ số P/W nhỏ nhất 





Cơ cấu chấp hành thủy lực sử dụng dâu để cung cấp năng lượng cho các bộ 
phận robot. Cơ cấu chấp hành thủy lực có tỷ số P/W lớn, có khả năng sinh lực 
và mômen lớn ở tốc độ thấp đối với chuyển động tịnh tiến và quay, nên thường - 
được sử dụng cho trong các robot cỡ lớn ở các dây chuyên sản xuất ôtô thế kỷ 
trước, ví dụ robot Cincinnati Milcron T3. Tuy nhiên do một số nhược điểm như 
đã mô tả ở bảng 8.1, hiện nay cơ cấu thủy lực ít được sử dụng trong robot. 
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Cơ cấu chấp hành thủy lực có hai dạng cơ bản đơn giản nhất: có cấu 
xilanh-piston (hình 8.2a) sử dụng cho các khớp tịnh tiến và cơ cấu van quay 
(hình 8.2b) truyền động cho các khớp quay. 


Trục khớp 






Piston 


Lỗ dầu ra 





a) b) 


Hnh 8.2. Cơ cấu chấp hành thủy lưc: 

a- Cơ cấu xilanh-piston; 
b- Cơ cấu van quay. 

Một cơ cấu xilanh-piston có thể sinh ra lực tính theo công thức: 

F=p.A (8-4) 
trong đó: A - diện tích tác dụng của piston; 
P - áp suất làm việc của cơ cấu, thông thường p = 70 kg/cm?.m. 
Một cơ cấu van quay mô tả trên hình 8.3. 


T; 


Đầu ra 





Đầu vào Hinh 8.3. Cơ cấu van quay. 


Mômen cơ cấu van quay được xác định như sau: 
ụ p p l sẻ 
M= prdA = [prtdr =pt [ra sẽ p1Œ3 —r/) (8-5) 
1 " n 
trong đó: p là áp suất; t là độ đày hoặc độ rộng của xilanh; rạ, r; tương ứng là 
bán kính trong và ngoài của xilanh quay. 
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Lưu lượng đầu trong cơ cấu thủy lực được tính theo công thức: 


dV mxd?dx md?, 
=—=——--=— 8-6 
"ấm na. vào, 


2 
với: dV = “Thúy - vị phân thể tích của xilanh; 

dx là độ dịch chuyển của xilanh và % là tốc độ dịch chuyển của xilanh. 

Từ (8-6) thấy rằng, có thể điều khiển dịch chuyển và tốc độ của xilanh 
bằng điều khiển lưu lượng dầu trong xilanh. 


§.2.2. Hệ thống cơ cấu chấp hành thủy lực 

Hệ thống cơ cấu chấp hành thủy lực gồm các bộ phận sau: 

+ Xilanh thủy lực dịch chuyển tịnh tiến hoặc quay. Các cơ cấu này được 
điều khiển bằng các van secvo hoặc các van vận hành bằng tay để sinh lực, 
mómen làm chuyển động các khớp. 

+ Một bơm thủy lực là bơm áp lực cao cung cấp chất lỏng áp lực cao cho 
hệ thống. 

+ Động cơ điện truyền động cơ cấu bơm thủy lực. 

+ Hệ thống làm mát. 

+ Bình chứa lưu giữ chất lỏng cung cấp cho hệ thống và là bộ phận thu hồi 
chất lỏng thừa hoặc chất lỏng hồi lưu từ xilanh. 

+ Van secvo điều khiển lưu lượng chất lỏng cấp cho xilanh để điều khiển 
tốc độ dịch chuyển của cơ cấu khớp. Thông thường van secvo được truyền động 
bởi động cơ secvo. 

+ Các thiết bị kiểm tra an toàn. 

+ Các thiết bị, ống nối đẫn chất lỏng đi về. 

+ Các cảm biến sử dụng cho điều khiển chuyển động của xilanh như cảm 
biến vị trí, tốc độ, từ... 

Sơ đồ khối hệ thống cơ cấu chấp hành thủy lực điển hình được trình bày 
trên hình 8.4. Cơ cấu xilanh hai chiêu được cấp dầu và điều khiển bằng một cơ 
Cấu van secvo. Van secvo có cấu tạo như trên hình 8.5 được truyền động từ một 
động cơ secvo. Động cơ secvo được điều khiển bởi hệ thống truyền động điều 
khiển vị trí với tín hiệu phản hồi là vị trí cửa piston được đo nhờ cảm biến VỊ trí. 
Động cơ chỉ cần sinh một lực nhỏ để di chuyển piston của van secvo, từ đó điều 
chỉnh được lưu lượng và hướng của đường dầu cung cấp cho xilanh và điều 
khiển được tốc độ và hướng dịch chuyển của xilanh. 
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Van secvo 






LÑ 
‹<> —=\ _| 
II 






Lượng đặt 


Hình 8.4. Sơ đồ khối hệ thống chấp hành thủy lực. 





Hình 8.5. Cấu tạo cơ cấu secvo van. 


Cơ cấu van secvo gồm một van 2.ngăn với hai đường dâu hồi về và một 
đường cung cấp dâu. Piston của van được dịch chuyển nhờ một động cơ secvo. 
VỊ trí piston của van secvo ở sơ đồ là vị trí nghỉ là việc. Tùy theo chiều quay 
động cơ, piston van sẽ dịch chuyển lên hoặc xuống, dẫn đến cấp dầu cho xilanh 
dịch chuyển theo chiều lên, xuống tương ứng. Lưu lượng dầu cấp cho xilanh, do 
đó tốc độ dịch chuyển của xilanh được điều khiển bởi tốc độ quay động cơ. 

Các công thức (8-5) và (8-6) biểu diễn quan hệ mômen, tốc độ của cơ cấu 
chấp hành với lưu lượng dầu trong xilanh. Bằng điều khiển lưu lượng có thể 
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điều khiển được mômen hoặc tốc độ của cơ cấu. Trên hình 8.6 là sơ đồ hệ thống 
điều khiển hệ thống điều khiển với tín hiệu phản hồi vị trí, trong đó sơ đồ hình 
8.6 a là sơ đồ nguyên lý hệ thống và hình 8.6b là sơ đồ cấu trúc điều khiển. 


Bộ khuếch 
U đại secvo 
h ạ 





Cảm biến 


Hình 8.6. Hệ thống điều khiển cơ cấu thủy lực: 
a- Sơ đồ nguyên lý; 
b- Sơ đồ cấu trúc. 


Cơ cấu chấp hành (CCCH) là xilanh sẽ biến đổi lưu lượng dầu Q cấp từ van 
secvo thành tốc độ dịch chuyển & của piston. Khâu van secvo được mô tả gồm 
hai phần: Khâu khuếch đại K, biểu thị tỷ lệ tín hiệu điều khiển động cơ secvo 
(u,) và lưu lượng đầu ra; Khâu ma sát tĩnh của cơ cấu K /A. Bộ điều khiển vị trí 
với cấu trúc tỷ lệ (P) khuếch đại sai lệch giữa tín hiệu đặt U,„ và tín hiệu phản 
hồi vị trí U,. : 

Gần đây, một dạng khác của cơ cấu thủy lực được chế tạo là cơ cấu tay 
robot mềm. Cơ cấu có dạng như một cơ bắp người như hình §.7a. Nó gồm một 
túi dạng ovan đặt bên trong một vỏ có khả năng trượt. Khi áp suất trong túi 
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tăng, túi sẽ phồng lên và chiều dài ngắn lại như một bắp cơ. Trên hình 8.7b là 
một cơ cấu khớp sử dụng 2 “bắp cơ” ở hai phía ngược nhau. Khi áp suất trong 
một ““ bắp” tăng, nó sẽ phồng lên và ngắn lại dẫn đến trục của khớp sẽ quay về 
phía “bắp” đó. Góc quay của khớp được tính theo: 
=1 =1; 
0= = (8-7) 
trong đó: l¡, l„ là chiều dài của hai “bắp cơ” và r là bán kính của bánh răng. 


b) 





Hình 8.7. Cơ cấu tay robot mềm: 
a- cấu tạo cơ cấu; b- một cơ cấu khớp. 


8.3. Cơ cấu chấp hành khí nén 


Cơ cấu chấp hành khí nén có nguyên lý hoạt động tương tự như cơ cấu thủy 
lực. Nguồn năng lượng cung cấp và truyền động cho xilanh quay hoặc thẳng là 
dòng khí nén áp lực cao điều khiển bằng van điện. Do nguồn cung cấp khí nén 
đặt độc lập với cơ cấu chuyển động nên hệ số quán tính của hệ thống nhỏ, tuy 
nhiên các thiết bị khí nén hoạt động với áp suất thông thường là 7 — 8 at nên tỷ 
số P/W thấp hơn nhiều so với cơ cấu thủy lực. 

Một cơ cấu khí nén gồm một van được đóng mở bằng điện cấp khí cho 
xilanh. Khi cuộn dây của van có điện, van mở và dòng khí áp lực cao đi vào 
xilanh, lực sinh ra bởi piston được xác định tương tự như (8-4): 

F=p.A 
Khi van đóng, piston phục hồi về vị trí ban đầu, van hồi khí nén mở. 
Cơ cấu khí nén thường được sử dụng cho cơ cấu kẹp của tay của tay robot. 
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Cơ cấu kẹp kiểu khí nén có khả năng phản ứng tốt với các lực bên ngoài và do 
đó có khả năng thích ứng với các tác động không dự đoán trước được của môi 
trường. 

Nhược điểm của hệ thống khí nén là đặc tính động không tốt so với cơ hệ 
thống truyền động điện và hệ thống truyền động thủy lực. Một trong những yếu 
tố ảnh hưởng chất lượng động là thời gian trễ +. Thời gian trễ có quan hệ với 
chiều dài ống theo biểu thức sau: 


=.ÍÊ. ề 
cung (8-8) 


trong đó: p - mật độ không khí (kg/cm)); 
Ñ =x.P - môđun tải với x = Cz/Cy = 1,4; 
P - áp suất tuyệt đối. 
Thông thường, ở nhiệt độ bình thường: t/1 = 3 (ms/m). 
Trong khi đó cơ cấu thủy lực có 1/1 = 0,7 (ms/m). Vì vậy cơ cấu thủy lực có 
tốc độ dịch chuyển lớn gấp 4 lần tốc độ dịch chuyển khí nén. 


+ 
l 


§.4. Động cơ điện 


Các đạng động cơ điện sử dụng trong hệ thống truyền động robot là: Động 
cơ secvo một chiều, động cơ secvo xoay chiều, động cơ secvo một chiều không 
chối than và động cơ bước. 

Động cơ nam châm vĩnh cửu (Parmanent Magnet — PM) sẽ phát nhiệt ít 
hơn vì không có cuộn dây kích từ. Vấn để thoát nhiệt cho động cơ được quan 
tâm đặc biệt khi thiết kế động cơ. Nhiệt sinh ra trong động cơ chủ yếu gây ra 
bởi tổn hao đồng trong dây quấn (RÏ?); ngoài ra còn gây ra bởi tổn hao trong lõi 
thép, tổn hao ma sát, tổn hao ở chổi than. Khi thiết kế cần phải tạo ra đường 
thoát nhiệt sao cho nhiệt tiêu tán nhanh nhất, tránh sự phát nóng quá mức cho 
động cơ. 

Xem xét mô hình động cơ một chiều và xoay chiểu như hình 8.8. Ở động 
cơ, dây quấn phần ứng ở roto có dòng lớn, nên nhiệt chủ yếu gây ra ở rôto. 
Nhiệt sẽ phải thoát ra qua khe hở không khí, qua nam châm vĩnh cửu ở stato ra 
môi trường, vì vậy quá trình phát nhiệt xảy ra chậm. Trong khi đó, ở động cơ 
xoay chiều, dây quấn ở stato, nên sự tỏa nhiệt xảy ra nhanh hơn. Động cơ xoay 
chiều có thể làm việc với dòng stato cao hơn và do đó sinh công suất lớn hơn 
với cùng cỡ như động cơ một chiều. Sự tổa nhiệt động cơ bước tương tự như 
động cơ xoay chiều. 


221 









Khe hở 
không khí 


Khe hở 
không khí 


Vỏ stato Vỏ stato 


Nam châm 
Vĩnh cửu 


a) b) 


Hình 8.8. Mô tả phần thoát nhiệt của động cơ: 
a- Động cơ một chiều nam châm vĩnh cửu; 
b- Động cơ xoay chiều. 
8.4.1. Động cơ một chiều nam châm vĩnh cửu 
Từ trường ở stato của động cơ một chiều nam châm vĩnh cửu do nam châm 
vĩnh cửu:-có mật độ cao tạo ra. Do đó động cơ một chiều nam châm vĩnh cửu có 
trọng lượng nhỏ và bán kính nhỏ hơn động cơ một chiều nam châm điện. Từ 
trường stato không đổi nên động cơ một chiều nam châm vĩnh cửu dễ dàng đảo 
chiều chỉ bằng đảo chiêu điện áp phần ứng. 
Mômen động cơ tỷ lệ với dòng điện phần ứng: 

M=K„ạI (8-9) 
với K,„ - hằng số mômen có giá trị không đổi phụ thuộc vào mật độ từ thông của 
nam châm điện. 

Ở trạng thái làm việc xác lập, phương trình điện áp có dạng: 
-U=RI+K,© (8-10) 
trong đó: R - điện trở phần ứng; 
K, - hệ số sức điện động; 
œ - tốc độ góc động cơ. 
Kết hợp (8-9) và (8-10), phương trình mômen động cơ được viết ở dạng: 

M= X»U - ch 

R R 
Quan hệ M(o) là đường thẳng khi Ư = hằng số. Do mômen tỷ lệ với tốc độ 
nên có thể viết (8-11) ở dạng: 


M(œ) = ¡| ¬ (8-12) 


(8-11) 


K : 
trong đó: M, = _ - mômen ngắn mạch; 
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TA = K - tốc độ góc lớn nhất. 


Ẹ 


Công suất động cơ ở các tốc độ khác nhau được tính theo biểu thức sau: 


(@ 


P(®) = M(œ).o = M„sÍ TS) (8-13) 
Công suất động cơ lớn nhất sẽ 
tương ứng ở giá trị tốc độ œ`” xác định ® 


bằng cân bằng đạo hàm công suất theo ®%zs 





tốc độ góc với 0: 
4P(G) _ Á- ° }-Jtee=a 
dœ my | ø` 
„1 ' 
S3UYT4: @ =—@ 
ỳ 2 max 


Như vậy công suất động cơ đạt giá 
trị lớn nhất ở tốc độ bằng một nửa tốc độ  ọ 
lớn nhất. Hình 8.9 là đặc tính mômen và THÊ 7 HÒA Ăn 
công suất của động cơ một chiều nam Hinh 8.9. Đặc tính mômen và công suất. 
châm vĩnh cửu. 

Một trong những hướng nghiên cứu hiện nay là giảm tối thiểu quán tính 
của cơ cấu chấp hành, nhằm tăng gia tốc và mômen của hệ thống. Một dạng 
động cơ một chiều mới đã được nghiên cứu chế tạo và ứng dụng trong robot là 
động cơ đĩa. Trong động cơ đĩa, ở rôto không có lõi thép, từ đó giảm đáng kế 
khối lượng của động cơ. Rôto động cơ có cấu tạo như một đĩa đẹt, các thanh 
dẫn đồng được tạo ra bằng cắt từ một phiến đồng và được gắn trên đĩa đó. Nam 
châm vĩnh cửu tạo từ trường gồm nhiều thỏi nam châm hình trụ nhỏ, ngắn được 
đặt hai phía của đĩa dây quấn stato như hình 8.10. Với cấu tạo của động cơ đĩa 
như vậy, hằng số thời gian cơ học nhỏ hơn 10 lần so với động cơ một chiều có 
lõi sắt thông thường, gia tốc của động cơ đĩa lớn gấp 10 lần và giá trị mômen 
đỉnh lớn gấp 2,5 lần so với động cơ một chiều nam châm vĩnh cửu thông 
thường. : 

Hệ thống truyền động secvo động cơ điện một chiều nam châm vĩnh cửu 
gồm 3 mạch vòng vị trí, tốc độ và dòng điện với bộ biến đổi thường sử dụng bộ 
điêu chỉnh điện áp một chiều (DC/DC) có sơ đồ khối cơ bản như hình 8.11. Cấu 
trúc hệ thống điều khiển, nguyên lý làm việc và phương pháp tổng hợp các 
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mạch vòng điều khiển đã được trình bày rất chỉ tiết trong các tài liệu và sách 
Truyền động điện tự động ([1], [2]). Người đọc có thể đọc các tài liệu đó để biết 
thêm chỉ tiết. 


Đĩa rôto 






Cảm biến 
vị trí 


Hình 8.11. Hệ thống điểu khiển động cơ điện một chiều. 


8.4.2. Động cơ đồng bộ nam châm vĩnh cửu 
Động cơ xoay chiều đồng bộ nam châm vĩnh cửu (ĐBNCVC) có rôto là 
một nam châm vĩnh cửu và dây quấn 3 pha ở stato như hình 8.12. Với cấu tạo 
như vậy, ở động cơ đồng bộ nam châm vĩnh cửu không có chổi than, nên gọi là 
động cơ xoay chiều không chổi than (brushless AC PM motor). Trong động cơ 
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` đồng bộ nam châm vĩnh cửu thường có kèm theo các cảm biến vị trí và cảm 
biến tốc độ được sử dụng cho hệ thống truyền động secvo. ĐBNCVC thường 
được cấp và điều khiển từ một 
bộ biến tần nguồn áp hoặc nguồn dòng với điều khiển tần số và điện áp 
theo quy luật yêu cầu. 
Mômen động cơ ĐBNCVC được xác định theo biểu thức (Krisnan) 


3P; 21 so 
. W;Ï§ ta 6+c hy —L)]§ sin 2ỗ (8-14) 
trong đó: 
I, tự; - tương ứng là dòng điện stato và Non điểm 
từ thông rôto trên trục; 


Lạ, Lụ, Lụ- điện cảm từ hóa, điện cảm 

đọc trục và điện cảm ngang trục; 

ỗ - góc giữa vectơ dòng i, và vectơ từ 

thông rôto, gọi là góc mômen. 

Khi õ = 90, mômen đạt giá trị lớn. 
nhất: 


3 
=- PW/1; (8-15) 





(8-15) cho thấy rằng, mômen động ,. s42, Cấu tạo động cơ ĐBNCVC. 
cơ ĐBNCVC tương tự như mômen động 


cơ một chiều và mômen động cơ không đồng bộ điều khiển vectơ. Mômen có 
thể được điều khiển bằng điều khiển biên độ dòng điện stato tương tự như điều 
khiển dòng phần ứng động cơ một chiều. Mômen động cơ dương khi  > 0, 
động cơ làm việc ở trạng thái động cơ; mômen động cơ âm khi ồ < 0, động cơ 
làm việc ở trạng thái máy phát. 

Trên cơ sở nguyên lý điều khiển mômen như trên, một số sơ đồ hệ thống 
điều khiển động cơ ĐBNCVC được trình bày trên hình 8.13. Hệ thống điều 
khiển mômen (hình 8.13a) chỉ bao gồm một mạch vòng điểu khiển mômen. 
Nguyên lý điều khiển dựa trên quan hệ trực tiếp mômen và dòng điện stato (8- 
15) khi từ thông roto không đổi. Sơ đỏ hình 8.13b là sơ đồ điều khiển tốc độ 
gồm hai mạch vòng điều chỉnh tốc độ với cấu trúc bộ điều khiển PI và điều 
. chỉnh mồmen. Tín hiệu phản hồi tốc độ nhận được từ cảm biến vị trí qua một 
khâu đạo hàm. Hệ thống điều khiển secvo vị trí như ở hình 8.13c, trong đó có 3 
mạch vòng vị trí, tốc độ, mômen. 
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Câm biến 
vị trí 








c) 


Hình 8.10. Một số hệ thống điều khiển động cơ ĐBNCVC: 
a- Hệ thống điều chỉnh mômen; 
b- Hệ thống điều chỉnh tốc độ; 
c- Hệ thống điều chỉnh vị trí. 
8.4.3. Động cơ một chiêu không chổi than 
Động cơ một chiều không chổi than (MCKCT) (Brushless DC Motor) có 
cấu tạo tương tự như động cơ ĐBNCVC. Rôto động cơ là nam châm vĩnh cửu. 
Dây quấn stato gồm 3 dây quấn tương tự như dây quấn động cơ không đồng bộ 
và đông cơ ĐBNCVC, chỉ khác dây quấn mỗi pha bố trí tập trung trên bể mặt 
stato với độ rộng 2x60° như hình 8.11a. Dây quấn sato có thể được nối theo 
hình sao hoặc tam giác. 
Dây quấn stato được cấp điện từ một bộ chuyển mạch điện tử có cấu tạo 
tương tự như mạch nghịch lưu của bộ biến tần như sơ đồ hình 8.12. Sức điện 
động cảm ứng trên dây quấn stato e, bởi sự quét của từ trường nam châm vĩnh 
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cửu rôto có dạng hình thang với 
độ rộng 1200 điện (hình 8.13a-c) 
có biên độ tính theo công thức 
Sau: 

E, = Kẹo, (8-16) 
với Kg - hằng số tỷ lệ với biên độ 
từ thông rôto; 

œ, - tốc độ quay của rôto. 

Nếu dòng điện stato có biên : 
độ không đổi và cung cấp cho  Lõisắtrôto 
các dây quấn stato ứng với 
khoảng sức điện động đạt giá trị 
không đổi như hình 8.13d, công suất tức thời của pha tương ứng là: P, = e,.. l, có 
dạng như hình 8.13g. Công suất động cơ là tổng của công suất 3 pha sẽ không 
đổi theo thời gian (hình 8.13k) và mômen động cơ được tính theo biểu thức sau: 


M=Pa†Ps†+P. _ 2P, ` 2.ˆg@;d, 


Nam châm 
vĩnh cửu 






Hình 8.11. Cấu tạo động cơ MCKCT. 


=2.Kz1, (8-17) 





. Điều khiển chuyển mạch 


Hình 8.12. Sơ đồ bộ chuyển mạch điện tử. 
Từ (8-17), mômen động cơ có thể điều khiển bằng điều khiển biên độ dòng 


stato I, tương tự như điều khiển mômen động cơ điện một chiều tỷ lệ dòng điện 
phần ứng khi từ thông không đổi. 
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Để đảm bảo công suất động cơ và mômen động cơ không đối theo thời 
gian, đồ thị chuyển mạch dòng điện qua dây quấn các pha động cơ như đồ thị 
8.13 đ— 8.13f. Hình 8.14 biểu diễn các sơ đổ tương ứng với các trạng thái 
chuyển mạch của mạch nghịch lưu cấp dòng cho động cơ MCKCT. Để đảm bảo 
chính xác thời điểm cung cấp dòng cho động cơ các pha của động cơ theo đồ thị 
hình 8.13, hệ thống điều khiển tạo xung cần một cảm biến đo vị trí tạo tín hiệu 
vào cho khối tạo xung điều khiển chuyển mạch như hình 8.12 





(b) 
() 
(4) 
(e) 
(0 
(g) 
®) 
b 0 ‡ H ị ị ị 
Œ'. lộ nñ ẽố aả4 
1mm mm. mm n 


Hinh 8.13. Đồ thị các đại lượng của động cơ MCKCT. 
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5" nh ni 


Hình 8.14. Các trạng thái chuyển mạch của mạch nghịch lưu. 


8.4.4. Động cơ bước 

9ø. Cấu tạo và nguyên lý làm việc: Động cơ bước là loại động cơ vận hành 
và điều khiển đơn giản, được sử dụng cho những hệ thống đơn giản, không yêu 
cầu tín hiệu phản hồi vị trí. Rôto động cơ bước là nam châm vĩnh cửu. Stato 
động cơ là các cuộn dây, thông thường có bốn nửa cuộn dây ó. ¡ thành hai cuộn 
dây. Sơ đồ nối các cuộn dây stato điển hình như hình 8.15. Hai cuộn dây Á và B 
chia làm hai nửa cuộn tương ứng là A;,, A; và Bạ, B;. Các nửa cuộn dây nối 
ngược nhau, đo đó sẽ tạo ra từ trường ngược nhau khi nửa cuộn tương ứng được 
cấp nguồn. 

Động cơ bước hoạt động trên nguyên tắc: Khi có một xung cung cấp cho 
cuộn dây stato, roto sẽ quay một bước, 


độ lớn của góc trên một bước phụ thuộc À& 
vào số nam châm ở rôto. Các động cơ 2 
bước được chế tạo với số bước ứng với A, 
một vòng quay là 12; 24; 72; 144; 180; B 


200. Tương ứng góc quay một bước là: 


+Ùy 
30”; 15”: 5: 2,5% 2°: 1,8, Phổ biến là 
các động cơ 1,8°/bước, đặc biệt có động ° 
cơ có số bước là 10 000 bước/vòng. 


Xét nguyên lý làm việc cơ bản của 
động cơ bước có hai cuộn dây stato và — Hình 8.15, Sơ đồ nối dây động cơ bước. 
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một nam châm ở rôto như hình 8.16. Giả thiết cuộn Bị và A; được cấp điện (có 
xung , và $;), động cơ ở trạng thái ban đầu, rôto sẽ nằm ở vị trí khi cực tính từ 
trường stato và rôto ngược nhau (hình 8.16a) và giữ nguyên trạng thái cân bằng 
khi không có sự thay đổi trạng thái cấp điện ở cuộn dây stato. Tiếp theo, cuộn 
A¡ bị mất nguồn, cuộn A; được cấp nguồn (không có xung ở, và có xung j;). Từ 
trường do cuộn dây A tạo ra ngược lại, rôto quay một góc 90? đến vị trí mới 
(hình 8.16b). Tương tự, khi một cuộn dây bị cắt nguồn và một cuộn khác được 
cấp nguồn, rôto tiếp tục quay như minh họa trên hình (8.1óc, đ). Nếu trình tự 
cấp xung vào các cuộn dây đảo lại, động cơ sẽ quay theo chiều ngược lại. 


+=——— 


& Z7 NN⁄/ Z7 S ` N>⁄ ⁄7 N 
. S N 
Bước †1 ? Bước 2 Bước 3 Bước 4 
a) b) c) đ) 


Hình 8.16. Nguyên lý làm việc cơ bản của động cơ bước. 


Động cơ bước có thể làm việc ở hai chế độ: quay đủ bước (full step) và 
quay bước một nửa (half step) tùy theo phương pháp điều khiển cấp điện các 
cuộn dây stato. Động cơ làm việc ở chế độ bước đủ khi ở mỗi trạng thái làm 
việc của động cơ, luôn có hai cuộn dây stato được cấp nguồn như minh họa ở 
bảng 8.2. Khi đó động cơ sẽ quay một góc 90” ứng với một bước như hình 8. l6. 


Bảng 8.2. Điều khiển ở chế độ quay bước đủ 


[ Chiẻquy | Bướ | ò | b | b% | k—_ 


Có Không Có 
Có Có 
Có 







Thuận Không 


Không Không 


xuân 


Có 
Có Không Có 
Có Không Không 
Ngược Có Không "hà Có 
Không Có “% n 
Không Có 
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b mm Ì+ CG) bò 


+® t0 


Động cơ làm việc ở chế độ quay bước một nửa khi ở trạng thái làm việc 
trung gian của động cơ (bước một nửa) chỉ có một cuộn dây stato được cấp 
nguồn như minh họa ở bảng 8.3. Khi đó động cơ sẽ quay một góc 45 ứng với 
một bước. Hình 8.17 minh họa các trạng thái làm việc của động cơ từ bước 1 
đến bước 2,5 


Bảng 8.3. Điều khiển ở chế độ quay bước một nửa 


Chiều quay 






Hình 8.17. Các trạng thái làm việc của động cơ ở chế độ quay nửa bước. 
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b. Đặc tính của động cơ bước: Đặc tính 
tốc độ - mômen động cơ bước được biểu = 
diễn trên hình 8.18. Động cơ sinh mômen 
lớn nhất khi tốc độ động cơ bằng không 
(rôto ở trạng thái đứng yên). Khi tốc độ tăng, 
mômen sẽ giảm. Đây là một đặc điểm quan 
trọng cần quan tâm đặc biệt khi thiết kế hệ 
thống. Hệ thống điều khiển động cơ bước 
không yêu cầu mạch phản hồi. Tuy nhiên, 
trong một số trường hợp có thể xảy ra tượng mất bước, nếu không dùng phản 
hồi vị trí sẽ gây sai số điều khiển. 

Động cơ mất bước khi mômen tải quá lớn hoặc động cơ quay tốc độ quá 
lớn. Tương ứng với một xung điều khiển mội góc bước, động cơ phải trải qua 3 
giai đoạn: gia tốc, ồn định, giảm tốc. Nếu tần số xung quá lớn, do quán tính 
động cơ lớn, động cơ sẽ không có đủ thời gian để thực hiện một góc bước. Điều 
đó đồng nghĩa với hiện tượng mất bước. Vì nguyên nhân này, tốc độ động cơ sẽ 
bị hạn chế. 

Một nguyên nhân của hiện tượng mất bước là sự suy giảm chậm của từ 
trường cuộn đây stato khi mất nguồn. Từ thông suy giảm chậm do sức điện 
động tự cảm sinh ra trong cuộn dây khi bị cắt nguồn. Để giảm quán tính mạch 
với các cuộn đây để tạo mạch giải phóng năng lượng cho các cuộn dây như hình 
8.19. 


M 


Hình 8.18. Đặc tính tốc độ - mômen. 





Hình 8.19. Sơ đồ nối dây động cơ bước có mạch triệt từ dư. 


c. Điều khiển động cơ bước: Động cơ bước có thể điều khiển bằng vi xử lý 
ghép trực tiếp hoặc ghép qua một mạch khuếch đại tùy theo công suất của động 
cơ bước. Sơ đồ điều khiển ghép trực tiếp không có mạch khuếch đại (hình 
8.19a) sử dụng khi vi xử lý có khả năng chịu dòng lớn. Khi vi xử lý có dồng đầu 
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ra nhỏ, động cơ bước được nổi với vi xử lý thông qua các mạch khuếch đại dùng 
tranzito như hình 8.20b. 


+Ù,, 





+Ù 





Hinh 8.20. Sơ đồ mạch điều khiển động cơ bước. 


b) 


Một dạng sơ đồ khác sử dụng vi mạch tích hợp chuyên dụng điều khiển 
động cơ bước. Hai tín hiệu đầu vào vi mạch chuyên dụng là đầu ra vi xử lý là 
dãy tín hiệu xung điều khiển với tần số xác định và tín hiệu quy định chiều 
quay. Nếu vi mạch có đòng đầu ra lớn, có thể ghép nối trực tiếp với các cuộn 
dây stato động cơ. Ví dụ, hãng Allegro Microsystems Inc. có các vi mạch 
SLA7024M, SLA7026M, SLA7029M, SLA7042M và SLA7044M với đòng 
định mức từ 1 - 3 A. Nếu vi mạch có dòng đâu ra nhỏ, cần sử dụng các tranzito 
công suất ghép nối với các cuộn dây động cơ bước. Hãng Motorola có vi mạch 
tích hợp công suất nhỏ MC3479 và SGS-L297 ghép nối với mạch khuếch đại 
tranzito SGS-L298. Hình 8.21 là một sơ đồ điều khiển động cơ bước sử dụng 
các vi mạch của Motorola. 
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Hinh 8.21. Sơ đồ mạch điều khiển động cơ bước sử dụng vi mạch Motorola, 


1 2 3 4 5 67 68 12 34 5 
Step clock 


Phase A [ Ỉ Ï | 


Phase B 


Phase C 


Phase D 
Uạn 


Ucp 





Hình 8.22. Dạng xung ở chế độ điều khiển nửa bước. 


Khi tín hiệu Half/Full có mức cao (Half = “1'), hệ thống điều khiển phát 
xung điểu khiển động cơ ở chế độ nửa bước. Tín hiệu vào và ra các vi mạch 
L297 và 1/298 có dạng như hình 8.22. Khi tín hiệu Half/Full có mức thấp (Half 
= '0`), hệ thống điều khiển phát xung điều khiển động cơ ở chế độ bước đủ. Tín 
hiệu vào và ra các vi mạch L297 và L298 có đạng như hình 8.23. 
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Phase A Ỉ l Ỉ | ị | Ỉ 
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Phase C 


Sẽ xxx. 


Uạp | | | | | 
Hình 8.23. Dạng xung ở chế độ điều khiển bước đủ. 


8.5. Cấu hình phần cứng hệ thống điều khiểu 
8.5.1. Cấu hình hệ thống điều khiển chung robot 


Sơ đồ cấu hình phần cứng thông dụng của một robot công nghiệp được mô 
tả trên hình 8.22. Sơ đồ gồm một phân cứng hệ thống (system board) và phân 
cứng hệ thống điều khiển secvo - cơ cấu chấp hành ghép nối thông qua một 
đường truyền (Bus). Phần cứng hệ thống có cấu trúc dạng CPU gồm: một vì xử 
lý với một bộ đồng xử lý toán học; bộ nhớ EPROM; bộ nhớ RAM; bộ nhớ 
RAM dùng chung với các hệ thống khác; các cổng nối tiếp và song song ghép 
nối Bus với thiết bị ngoại vi như máy in, bàn phím...; bộ đếm, bộ ghi dịch, bộ 
thời gian; hệ thống ngắt. 

Khâu HỆ THỐNG (System Board) thực hiện các chức năng sau: Giao tiếp 
với người vận hành thông qua bàn phím, bàn phím dạy học, máy in và video; 
ghép nối với bộ nhớ ngoài (đĩa cứng) để lưu trữ đữ liệu và chương trình thông 
qua mạng truyền thông cục bộ; ghép nối vào/ra với các thiết bị ngoại vị trong 
môi trường làm việc của robot như băng tải, các cảm biến...; máy lập chương 
trình và các đường BUS. Hệ thống có thể truyền thông với các vì xử lý của hệ 
thống khác để thực hiện chức năng tính toán phụ khác. Khâu ĐỘNG HỌC RB 
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thực hiện tính toán các đữ liệu chuyển động ban đầu; tính toán động học vị trí 
thuận, ngược và ma trận Jacobien... Khâu ĐỘNG LỰC HỌC RB thực hiện tính 
động lực học ngược. Khâu ĐK SECVO có chức năng tính toán nội suy lượng 
đặt; tính toán các luật điều khiển; biến đổi số - tương tự và ghép nối với các 
khâu KĐCS của các hệ truyền động các khớp; đọc và lưu trữ các số liệu từ cảm 
biến vị trí và tốc độ; ngắt các chuyển động khi có sự cố. Khâu ĐO LỰC thực 
hiện chức năng thu nhận số liệu từ cảm biến lực và tính toán lực ở khung toạ độ 
đã cho. Khâu THỊ GIÁC tính toán và xử lý dữ liệu từ các camera, tách các đặc 


trưng hình học từ các hình ảnh thu nhận được và đặt đối tượng ở các khung toa 
độ đã cho. 


Bộ nhớ Máy in Bàn phím Video Bàn 
ngoài dạy học phím 


TT 
làm việc 
l ĐỘNG HỌC IĐỘNG LỰC 


| 


ĐK SEGVO ĐO LỰC 


Cảm biến Động cơ Cảm biến 
vị trí, tốc độ Se@Cvo lực 


Hình 8.24. Sơ đồ cấu hình phần cứng hệ thống điều khiển robot công nghiệp. 
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Các khâu được nối chung với BUS chung của hệ thống, nhưng tân số mẫu 
của các khâu trong hệ thống không như nhau. Thông thường, các khâu nối với 
các cảm biến có tần số lấy mẫu cao nhất (100 - 1000 Hz) để đâm bảo chất lượng 
động chuyển động của tay robot cao nhất. Khâu ĐỘNG HỌC RB và ĐỘNG 
LỰC HỌC RB không yêu cầu cập nhật dữ liệu nhanh như các khâu ĐK 
SECVO. Trong thực tế, cấu hình robot cũng không thể thay đổi trong thời gian 
quá ngắn do quán tính của hệ thống. Thông thường tân số mẫu của các khâu đó 
là 10-100 Hz. Tương tự khâu THỊ GIÁC có tần số mẫu thấp nằm trong khoảng 
1-10 Hz. 


8.5.2. Cấu hình hệ thống điều khiển secvo khớp 

Hệ thống điều khiển sevo khớp là hệ thống điều khiển vị trí vòng kín có sơ 
đồ như hình 8.23. Hệ thống điều khiển secvo nhận tín hiệu đặt là vị trí và tốc độ 
đặt từ vi xử lý và các tín hiệu phản hồi vị trí và tốc độ khớp từ các cảm biến gắn 
trên trục khớp hoặc trục động cơ; thực hiện tính toán tín hiệu điều khiển từ các 
sai lệch vị trí và tốc độ với các luật điều khiển thiết kế trước và điều khiển cơ 
cấu chấp hành thông qua bộ khuếch đại cồng suất. 


Tín hiệu điều khiển 
cơ cấu khớp robol 







Cơ cấu truyền 
động khớp. 







Tín hiệu phẳn 
hồi tốc độ 












Tín hiệu phần 
hồi vị trí 






Tín hiệu 
đặt vị trí. 





Cảm biến vị trí 
và tốc độ 


Hình 8.25. Sơ đồ cấu hình hệ thống điều khiển secvo khớp. 
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CHƯƠNG 9 
CẢM BIẾN TRONG ROBOT 


Cảm biến trong robot có thể chia làm hai loại: cảm biến trạng thái bên 
trong và cảm biến trạng thái bên ngoài. Cảm biến trạng thái bên trong có chức 
năng đo lường các biến, tham số như vị trí khớp, tốc độ khớp được sử dụng cho 
các mạch vòng điều khiển trong hệ thống điều khiển robot. Cảm biến trạng thái 
bên ngoài sẽ đo lường các biến khoảng cách, mức, tiếp xúc. Cảm biến trạng thái 
ngoài có thể phân làm hai loại: tiếp xúc và không tiếp xúc. Cảm biến tiếp xúc sẽ 
phản ứng với sự tiếp xúc vật lý như lực, độ trượt, mômen. Cảm biến không tiếp 
xúc sẽ phản ứng với các tia điện từ như các cảm biến khoảng cách, mức... 

Sử dụng cảm biến bên ngoài cho phép robot tiếp xúc với môi trường một 
cách mềm dẻo và thông minh hơn. Điều này sẽ khác với các hoạt động lập trình 
trước, các rôbot được “dạy” làm theo các thao tác lặp đi lặp lại, thông qua các 
chức năng được lập trình trước. Với các cảm biến bên ngoài, các robot có thể 
“nhìn” và “ cảm giác” để thực hiện các nhiệm vụ phức tạp trong khi cần ít hơn 
các cơ cấu điều khiển hơn các robot lập trình sẵn. Đồng thời các robot cũng có 
khả năng thích nghỉ với các dạng công việc khác nhau, đạt được độ vạn năng 
cao hơn. Do đó sẽ góp phần giảm giá thành sản phẩm và bảo dưỡng. 

Trong chương 9 sẽ trình bày đặc tính các cảm biến trong robot và lần lượt 
mô tả cấu tạo, đặc tính của các cảm biến bên trong và bên ngoài sử dụng trong 
robot. 


9,1. Các đặc tính của cảm biến 

a. Dạng tín hiệu đầu ra (số hay tương tự): Tín hiệu ra của cảm biến có 
thể có đạng tương tự hoặc số tùy theo ứng dụng của cảm biến trong robot. Các 
tín hiệu đó có thể được sử dụng trực tiếp hoặc được biến đổi. Ví dụ, đầu ra của 
chiết áp là tín hiệu tương tự, tín hiệu ra của encoder là số. Encoder có thể nối 
trực tiếp với vi xử lý, tín hiệu ra của chiết áp phải được biến đổi nhờ biến đổi 
tương tự — số (A/D). 

b. Độ phân dải: Độ phân dải là giá trị bước nhỏ nhất trong phạm vì đo 
của cảm biến. Đối với chiết áp, độ phân dải tỷ lệ giá trị điện trở của một vòng 
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dây quấn. Với cảm biến số n bit, độ phân dải được tính theo biểu thức sau: 
Độ phân dải =Êm : (9-1) 
2n 


trong đó: R„ - giới hạn phạm vi đo. 

Ví dụ: encoder tuyệt đối 4 bít có độ phân dải là: 306/2*= 22,5°. 

c. Độ nhạy: Độ nhạy là tỷ số độ thay đổi đầu ra với độ thay đổi tín hiệu 
vào. Cảm biến có độ nhạy cao sẽ có tín hiệu ra thay đổi lớn khi có sự thay đổi 
tín hiệu vào kể cả nhiễu. 

d. Độ tuyến tính: Độ tuyến tính biểu thị quan hệ giữa mức độ thay đổi đầu 
vào và mức độ thay đổi đầu ra. Cảm biến có đặc tính đầu ra tuyến tính sẽ có 
mức độ thay đổi tín hiệu ra bằng mức độ thay đổi tín hiệu vào. Hầu hết các cảm 
biến có đặc tính đầu ra phi tuyến. Một số cảm biến có đặc tính tuyến tính trong 
một phạm vi nhất định. Nếu đầu ra là phi tuyến, nhưng biết được độ phi tuyến, 
có thể bù được bằng một mô hình chính xác, các phương trình bổ sung hoặc 
bằng một sơ đồ mạch điện tử. 

e. Giới hạn đo: Giới bạn đo là hiệu giữa giá trị lớn nhất x giá trị nhỏ nhất 
hoặc là hiệu giữa giá trị lớn nhất và giá trị nhỏ nhất trong phạm vi làm việc 
chính xác. 

f. Thời gian đáp ứng: Thời gian đáp ứng là thời gian cần thiết để đầu ra 
đạt tới tỷ lệ phần trăm của đâu ra lớn nhất, thông thường là thời gian để đầu ra 
tăng đến 95 % giá trị đầu ra tổng. Thời gian đáp ứng cũng có thể định nghĩa là 
thời gian cần thiết để quan sát được sự thay đổi đầu ra khi đầu vào thay đổi. Ví 
dụ thời gian đáp ứng của nhiệt kế thủy ngân dài, thời gian đáp ứng của nhiệt kế 
số đo nhiệt độ dựa trên nhiệt bức xạ có giá trị nhỏ. 

g- Đáp ứng tần số: Đáp ứng tân số là phạm vi tần số xảy ra cộng hưởng 
mạnh trong hệ thống đối với tín hiệu vào. Phạm vi đáp ứng tần số càng lớn, khả 
năng phản ứng của hệ thống với tín hiệu vào càng tốt hơn. Tương tự, cần phải 
xác định đáp ứng tần số của cảm biến và kiểm tra liệu đáp ứng của cảm biến có 

đủ nhanh ở mọi điều kiện làm việc không. 
: h. Độ tin cậy: Độ tin cậy là tỷ số tổng thời gian làm việc tốt của hệ cảm 
biến độ tin cậy cao trong khi cũng cần phải quan tâm đến giá cả và các yêu cầu 
cao. 

ï. Độ chính xác: Độ chính xác đánh giá mức độ đạt tới giá trị cân đo của 
cảm biến. Độ chính xác thông thường được xác định bởi giá trị sai lệch giữa giá 
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trị cân đo và giá trị thực tế đo được. Sai lệch càng lớn có nghĩa là độ chinh xác 
càng kém. 

k. Độ lặp lại: Đầu ra cảm biến ở một số lần đo có thể có giá trị khác nhau. 
Độ lặp lại cho biết phạm vì thay đổi của giá trị đo được trong một số lần đo hứu 
hạn. Nói chung, độ lặp lại có nghĩa quan trọng hơn độ chính xác, vì đa số 
trường hợp, sai số là đối xứng, có thể hiệu chỉnh hoặc bù được do có thể phán 
đoán hoặc đo được. Độ lạp lại hoàn toàn là giấ trị ngẫu nhiên không dễ bù được. 

I. Trọng lượng: Trọng lượng cảm biến sẽ ảnh hưởng đến trọng lượng 
chung của các bộ phận chuyển động nên ảnh hưởng đến quán tính chung của cả 
hệ thống robot. Từ đó làm giảm chất lượng động của robot. 

m. Kích thước: Các khớp robot là các bộ phận chuyển động, nên kích 
thước cảm biến lấp đặt trên khớp sẽ phải phù hợp với không gian lắp đặt của 
khớp. 

n. Giá thành: Giá thành là một yếu tố quan trọng và được Xem xét cùng 
với các chỉ tiêu khác khi thiết kế hệ thống robot như độ tin cậy, độ chính xác... 


9.2. Cảm biến vị trí 


Cảm biến vị trí được sử dụng để đo vị trí của các bộ phận chuyển động bao 
gồm chuyển động quay và tịnh tiến. Cảm biến vị trí có nhiều loại: dạng tương 
tự, số; dạng chiết áp, encoder, resolver.,. Cảm biến vị trí cũng có thể sử dụng để 
tính toán tốc độ của cơ cấu chuyển động, ví dụ như sử dụng encoder. 

9.2.1. Chiết áp 

Chiết áp biến đổi tín hiệu vị trí thành giá trị Rỳ 
điện trở đầu ra. Sơ đồ nối dây của chiết áp như x R ịu 
hình 9.1. Điện áp đầu ra tỷ lệ với điện trở ứng với 
vị trí con trượt, do đó tỷ lệ với vị trí của con trượt: - NẾ 

Hình 9.1. Chiết áp đo vị trí. 
0Ð 2 xi2: @-2) 
r n Ry n qgs 

Chiết áp đo vị trí có dạng chiết áp quay và dịch chuyển tịnh tiến, nên có thể 
được sử dụng đo góc quay của khớp quay và độ dịch chuyển tịnh tiến của khớp 
tịnh tiến. Thông thường chiết áp chế tạo bằng cuốn dây điện trở lên một lõi hoặc 
phủ một lớp vật liệu dẫn điện mỏng lên một bề mặt. Ưu điểm của chiết áp dạng 
phim mỏng là đầu ra sẽ thay đổi trơn, liên tục, không bị nhiễu. Từ đó có thể tính 
toán được tốc độ từ tín hiệu đầu ra của chiết áp. 
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Chiết áp được sử dụng tạo tín hiệu phản hồi vị trí cho các mạch vòng điều 
chỉnh vị trí của các hệ truyền động khớp. 
9.2.2. Biến áp vi phân tuyến tính 


Biến áp vi phân tuyến tính (BAVPTT) là một cơ cấu chuyển đổi đo địch 
chuyển. Cấu tạo BAVPTT gồm một cuộn dây sơ cấp và hai thứ cấp nối ngược 
cực tính nhau và một lõi thép đi động như hình 9.2. 


Eh, 


Dịch chuyển 


a) b) 





Hình 9.2. Biến áp ví phân tuyến tính: 
a- sơ đồ nối dây, b- đặc tính truyền đạt. 


Do hai cuộn dây thứ cấp nối ngược nhau nên điện áp ra có biên độ và pha 
thay đổi tùy thuộc vị trí của lõi thép di động. Khi lõi sắt ở vị trí giữa, điện áp 2 
cuộn đây có trị số bằng nhau và pha ngược nhau, điện áp ra bằng không. Khi lõi 
sắt dịch chuyển về một phía, điện áp một cuộn sẽ lớn hơn cuộn kia và điện ắp ra 
sẽ khác không và có pha đương. Khi lõi sắt dịch chuyển theo hướng ngược lại, 
pha của điện áp sẽ âm. Sử dụng khâu khuếch đại nhạy pha, điện án ra có giá trị 
và dấu phụ thuộc vị trí của lõi thép. Đặc tính của cơ cấu ở hình 9.2b. 


9.2.3. Resolver 


Resolver có cấu tạo và nguyên tắc làm việc tương tự như biến áp vi phân 
tuyến tính được sử đụng để đo góc quay. Resolver là một biến ấp, cuộn sơ cấp 
đặt ở phần quay và được nối vào nguồn điện xoay chiều. Hai cuộn thứ cấp đặt 
lệch nhau 90° ở phần tính. Trục cuộn dây sơ cấp quay theo cơ cấu, từ thông cơ 
cấu sẽ quay. Khi trục cuộn dây sơ cấp trùng với trục một cuộn, điện áp ra trên 
cuộn đó sẽ lớn nhất, điện áp trên cuộn kia sẽ nhỏ nhất và ngược lại. Như vậy 
điện áp ra trên một cuộn là hàm sin9, điện áp cuộn kia sẽ là hàm cos6. Độ lớn 
điện áp ra sẽ phụ thuộc vào góc lệch 9. Trên hình 9.3 là sơ đồ cấu tạo và dạng 
điện áp ra của resolver. 
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-_— 
Ti Ư = sinô 
-^o 
Hà 
U = cos6 


4) b) 
Hình 9.3. Sơ đồ nối dây dạng điện áp ra của resolver: 
a- sơ đồ nối dây; b- dạng điện áp ra. 


9.2.4. Thiết bị đo mã hóa quang học số (Digital optical Encoder) 

Thiết bị đo mã hóa quang học, gọi tắt là thiết bị đo mã hóa (encoder) là 
thiết bị biến đổi các thông tin chuyển động thành các tín hiệu xung. Bằng đếm 
một bit đơn hoặc giải mã đãy các bit, các xung có thể được biến đổi thành các 
vị trí tuyệt đối hoặc các vị trí tương đối. Encoder có hai loại: quay và tịnh tiến. 
Trong thực tế, phổ biến là các encoder quay. Encoder quay có hai dạng: encoder 
tuyệt đối (absolute encoder) và encoder tương đối (Increment encoder) 

Các encoder quay có cấu tạo gồm một đĩa thủy tỉnh hay nhựa với những dải 
— đen được dán hoặc khuếch tán vật liệu màu đen. Một cơ cấu phát và thu ánh 
sáng được đặt hai phía đĩa như hình 9.4. Phần đen sẽ cản ánh sáng, và đầu ra sẽ 
xuất hiện một xung. 





Xung đầu ra 
Hình 9.4. Cấu tạo của encoder. 
a, Encoder tuyệt đối: Encoder tuyệt đối sẽ mã hóa vị trí là góc quay thành 
dãy số nhị phân. Trên đĩa sẽ gồm nhiều rãnh sẽ tương ứng với số bit của 


encoder. Số lượng vị trí có thể phân biệt được là 2" (n là số rãnh trên đĩa). Mõi 
rãnh được phân bố số vạch đen, trắng theo mã nhị phân. Số trạng thái của 
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encoder 4 bit và số vị trí góc tương ứng sẽ là: 2* = 16. Độ phân dải đo góc hoặc 
sai số phép đo là: 
o- 6t” ~22,s° 
21 

Hình 9.5 mô tả sơ đồ cấu tạo của encoder trải trên mặt phẳng và dạng xung 
đầu ra của encoder 4 bịt. Các mã nhị phân tương ứng với các vị trí góc của 
cncoder 4 bit ở bảng 9.1. Số bit của encoder càng lớn, độ phân dải của encoder 
càng lớn, độ chính xác tương ứng càng cao. Ví dụ độ phân dải của encoder 8 
bit là: 





8= 360" =1,046° 
28 
Cảm biến 
cố định 
Bit3n 
Bit 2 a 
Bit+n 
Bit0H 
“.——————_ 
0 Chiều quay dương 30! 
B132 ——————T 


Bt2——T TL _—Ƒ—¬" 
Bti——ÍÏ L_.ÍF TL _T UUTT 
gto -ÍÏ LILILTLT LTLTLT— 


Hình 9.5. Sơ đồ và dạng xung đầu ra của encoder 4 bit. 


Bảng 9.4. Mã nhị phân và góc quay 


Mã thập Góc quay Mã nhị Mã n. Góc quay Mã nhị 
Án @® Hộ, @® mm 


0-22,5 180 - 202,5 






















` 225-45 202,5 - 225 
45-675 225 - 247,5 
67,5 - 90 247,5 - 270 
90 - 112,5 270 - 292,5 








112,5 -135 
135 - 1575 
157,5 - 180 


292,5 - 315 
315-3375 
337,5 - 360 
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c. Encoder tương đôi: gồm 3 rấnh, gọi là kênh, thông thường ký hiệu là A, 
BC (hoặc 2). Xung đầu ra kênh A, B lệch nhau 90°, kênh C thường có một 
xung. Hình 9.6 mô tả sơ đồ cấu tạo và dạng xung đầu ra các kênh của encoder 
tương đối. Số xung ứng với một vòng quay của encoder tùy theo các hãnh sản 
xuất thông thường có các giá trị: 360, 720, 1024... 


Cảm biến 
cố định 


Hình 9.6. Sơ đồ và dạng xung đầu ra của encoder tương đối. 


Bằng đếm số xung phát ra và dựa vào độ phân dải của encoder sẽ tính 
được góc quay. Chiều quay có thể được xác định bằng sử dụng xung kênh A, B 
lệch nhau 90° tùy thuộc vào sự vượt trước của xung À boặc B. Đếm số xung 
trong một đơn vị thời gian hoặc sử dụng xung kênh C sẽ tính được tốc độ. Xung 
kênh C cũng được sử dụng để xác định vị trí ban đầu. Hình 9.7 là các đồ thị 
xung đầu ra của encoder ở chiều quay thuận và chiều quay ngược. Sử dụng các 
sơ đồ.mạch lôgic có thể tạo ra các dãy xung thuận (CW), ngược (CCW) với độ 
phân dải khác nhau 1X, 2X và 3X. Ứng với độ phân dải 1X, chỉ có 1 xung 
trong một chu kỳ xung A, B. Tương tự, xung độ phân dải 2X sẽ có 2 xung 
trong một chu kỳ và 4 xung trong một chu kỳ với độ phân dải 4X. Như vậy độ 
chính xác đo sẽ cao hơn khi sử dụng xung 4X. Thuật toán tạo xung 1X có thể 
thực hiện đơn giản như sau: Xung CW xuất hiện khi A = Ỷ và B = 1; Xung 
CCW xuất hiện khi B = Ì và A = l; 

Sơ đồ đếm xung thuận và ngược được mô tả trên hình 9.8. Tín hiệu xung 
chiều thuận (CW) và chiều ngược (CCW) được tạo ta tương ứng bởi hai trigơ D. 
Hai dãy xung ở chiều thuận và ngược được đếm nhờ bộ đếm tiến và lùi. Đầu ra 
sẽ nhận được giá trị vị trí (góc quay) ở dạng mã nhị phân. Để tạo các xung 2X 
và 4X có thể thiết kế sơ đồ mạch lôgic bằng các phần tử rời rạc với thuật toán 
tương tự như xung 1X hoặc sử dụng các mạch chíp có sắn (ví dụ HCTL-2016 
của hãng Hewlett Packard) hoặc mạch vi xử lý. 
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QUAY THUẬN (CW) QUAY NGƯỢC (CCW) 
c P]e Ji I TƑ.” SmÌ 2J |: dị Si." 
|0 SIÍ 22 jt vi 3n], li 1.1 
œ— ]LTL— .—. 
s1 [] [1 [] Ƒ] 


8ù =——>—=. Li]Ì|LÌÙI1 


CCW 





——————-—————......... 


4X 


CCW LÍ | LÍ 


Hình 9.7. Dạng xung đầu ra ứng với chiều quay 
thuận và ngược của encoder tương đối. 


ĐẦU RA 
NHỊ PHẦN 





Hình 9.8. Sơ đồ đếm xung với độ phân. dải 1X. 


9.3. Cảm biến đo lực và mômen 

Cảm biến đo lực và mômen được sử dụng để đo phản lực sinh ra ở các nơi 
tiếp xúc giữa các chỉ tiết cơ khí. Gồm hai loại cảm biến đo lực và mômen cổ tay 
và khớp. Cảm biến khớp đo các thành phần trên các trục của lực và mômen tác 
dụng lên khớp robot và đặt chúng thành một vectơ lực khớp. Với các hệ truyền 
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động khớp sử dụng động cơ một chiều, đo mômen khớp thông qua đo dòng điện 
phần ứng động cơ. Cảm biến cổ tay gồm các cảm biến điện trở lực căng đặt ở cổ 
tay hoặc cơ cấu tác động cuối (end effector) đo sự biến dạng của các bộ phận 
của cơ cấu cổ tay khi có ngoại lực tác dụng. 

9.3.1. Cảm biến điện trở lực căng 

Cảm biến điện trở lực căng gồm một sợi kim loại mỏng, thông thường là 
constantan uốn cong hình ziczac được đán trên bề mặt một tấm polyme mỏng 
dễ biến dạng như hình 9.9a. Hai đầu dây kim loại được hàn trên một miếng kim 
loại to và dày để tạo thành đầu ra. Để đo lực tác dụng lên một cơ cấu, đán cảm 
biến lên thành của cơ cấu đó. Khi có ngoại lực tác dụng lên cảm biến làm cho 
miếng polyme biến đạng, điện trở của cảm biến sẽ thay đổi so chiều dài, tiết 
diện và điện trở suất của dây thay đổi. Do đó thông qua đo điện trở, sẽ tính được 
lực tác dụng lên cơ cấu. Để biến đổi sự thay đổi điện trở thành tín hiệu điện sử 
dụng sơ đồ cầu điện trở (cầu Weston) như hình 9.9b. Giá trị điện áp ra của cầu 
xác định theo biểu thức: 


- R; = R È 
¬ Kế, Đà Khổ 


Tấm Polyme Sợi kim loại 


Kết thúc 


Đầu ra 1 vòng 





8) 





Hình 9.9. Cấu tạo cảm biến điện trở lực căng và sơ đồ cầu điện trở. 
a- sơ đồ cấu tạo cảm biến; b- sơ đồ cầu điện trở. 


9.3.2. Cảm biến đo lực cổ tay robot 

Cơ cấu đo lực và mômen tác dụng ở tay robot có hình dạng như hình 9.10. 
Cơ cấu gồm một khung 4 thanh kim loại dễ biến dạng, trên mỗi thanh kim loại 
đặt 2 cảm biến điện trở lực căng. Mỗi điện trở cảm biến sẽ được nối vào một 
cầu đo điện trở như hình 9.9b, sẽ nhận được một tín hiệu điện áp ra tính theo 
(9.3). Số cảm biến lực căng là 8, tương ứng sẽ có 8 tín hiệu điện áp (wW, W;, W¿, 
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W, Ws, W2, W¿, W¿) lệ với lực tác dụng theo các hướng. Tám tín hiệu được đưa về 
máy tính, bằng phần mềm hiệu chỉnh lập trình sẵn sẽ tính được các thành phần 
lực và mômen tác dụng theo các trục tọa độ của tay robot. 

Ký hiệu vectơ lực tác dụng lên cổ tay robot là: 

TÉE=[F,,Fy,F,,M„,My M,]T (9-4) 

Với F,, Fy, F„ M,, M,, M, — tương ứng là các thành phần lực và mômen tác 
dụng theo trục X,y,z. 

Vectơ lực tác dụng lên cổ tay robot được tính theo công thức: 

T5E~R.W (9-5) 
trong đó: W - ma trận tín hiệu điện áp tỷ lệ điện trở lực căng nhận được từ các 
cảm biến: W = [WI,W2,Wj,Wa,Ws,Wg,Wj,Ws]f; 

R - ma trận hệ số hiệu chỉnh có dạng: 

0 0n 0 0 0 n 0 

m0 0 0 øœ 0 0 0 

0n 0 ng Ô0 nợ 0 ng (9-6) 

0 0 0 „ 0 0 0T 

0 mm 0 0 0 mg 0 0 

l@p Ô Ty Ô Tgg Ô my Ô 

Để tăng độ chính xác, có thể bổ sung thêm các thành phân của ma trận R. 


+ Thanh kim loại có đán 
cảm biến lực căng 






Hình 9.10. Cấu tạo cảm biến đo lực 
và mômen ở cổ tay: 
a- cơ cấu đo lực và mômen; b- các tín 
hiệu đầu ra cảm biến điện trở lực căng. 


247 


9.4. Cảm biến khoảng cách 


Cảm biến khoảng cách dùng để đo khoảng cách từ một điểm chuẩn, thông 
thường là điểm đặt cảm biến, đến đối tượng trong khu vực hoạt động của cảm 
biến. 

9.4.1. Kỹ thuật tam giác 

Phương pháp đo khoảng cách 
đơn giản được trình bày trên hình 
9.11. Chiếu một chùm ánh sáng đến 
bề mặt đối tượng. Chùm tia sáng 
phản xạ từ đối tượng đến bề mặt thu 
sẽ nằm trong một mặt phẳng. Nếu bộ 
phận thu được chùm tia sáng hội tụ ở 
một điểm, khoảng cách từ đối tượng 
đến bề mặt thu ánh sáng được xác 
định theo biểu thức: Hình 9.‡1. Phương pháp đo khoảng cách 

D=B.g6 (9-10) sử dụng kỹ thuật tam giác. 





với: B là khoảng cách từ nguồn sáng đến cơ cấu thu; 
Ø là góc giữa chùm tia sáng phát ra từ nguồn sáng đến đối tượng và đường 
nằm ngang. 


9.4.2. Phương pháp đo thời gian truyền một tỉa 

Nguyên tắc của phép đo là đo khoảng cách bằng đo thời gian truyền một tia 
từ nguồn sáng đến đối tượng và phản xạ về nguồn phát. Tia được sử dụng trong 
các cơ cấu đo khoảng cách là tia laze. Có hai phương pháp đo khoảng cách sử 
dụng tia laze (4Ì) 

Phương pháp đo khoảng cách thứ nhất sử dụng tia laze là đo thời gian 
truyền tia laze từ nguồn phát đến đối tượng và thời gian trở về điểm phát. 
Khoảng cách từ nguồn phát đến bên mặt đối tượng được xác định dựa theo tốc 
độ ánh sáng c và thời gian truyền xung T theo quan hệ: 

D=c.T2 (9-11) 

Vì tốc độ ánh sáng là c = 300 000 km/s, thiết bị điện tử đo khoảng cách 
phải có độ phân dải thời gian là 50 ps có thể đạt độ chính xác a = + 0,63 cm. 

Phương pháp đo khác sử dụng tia laze là đo thời gian trễ tức là đo góc địch 
pha giữa tín hiệu phát đi và tín hiệu thu được. Nguyên lý đo được trình bày trên 
hình 9.12a. Một chùm tia laze phát ra được chia làm hai chùm: chùm thứ nhất, 
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gọi là chùm tia chuẩn đựơc truyền tới cơ cấu đo pha; Chùm tia thứ hai truyền 
đến đối tượng và phản xạ về cơ cấu đo pha. Tổng khoảng cách chùm tia thứ hai 
đi là: 
D=L+2D (9-12) 
Chùm tia thứ hai truyền đi một khoảng cách dài hơn, do đó sẽ có sự dịch 
pha giữa chùm tia thứ nhất và chùm tia thứ hai như minh họa trên hình 9.12 b. 
Khi đó khoảng cách tổng được tính theo:  - 
D=L+„ 1Ã (9-13) 
360 . 
Kết quả (9-13) sẽ là duy nhất khi Ø < 360° hoặc 2D < ^2.. Kết hợp (9-12) và 
(9-13), nhận được biểu thức tính khoảng cách D: 


: s2) (9-14) 
360(2 





Hình 9.12. Phương pháp đo góc dịch pha: 
a- Nguyên lý đo dịch pha; 
b- Dịch pha giữa tín hiệu phát và tín hiệu phản xạ. 


Vì bước sóng của tia laze rất nhỏ (632,8 nm), phương pháp đo nói trên 
không thích hợp ứng dụng cho robot. Một phương pháp đo khác khắc phục vấn 
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đẻ này là điều chế chùm sóng laze bằng một tín hiệu có bước sóng cao hơn. Ví 
dụ tần số sóng mang là f = 10 MHz, bước sóng tia được truyền đi là A' = f2. = 
6,32 m. Hình 9.13 minh họa nguyên tắc này. 





Hình 9.13. Dạng sóng được điều chế. 


9.5: Cảm biến lân cận 

Cảm biến lân cận được sử dụng để phát hiện một đối tượng đang ở gần 
trong một phạm vi nào đó, Cảm biến lân cận giúp cho robot phát hiện vật cân để 
tránh hoặc phát hiện vật cần gắp... Một số loại cảm biến lân cận hoạt động dựa 
trên nguyên tắc cảm ứng, điện dung... 


9.5.1. Cảm biến lân cận kiểu cảm ứng 


Cảm biến lân cận kiểu cảm ứng hoạt động dựa trên nguyên lý điện cảm 
cuộn dây thay đổi khi có mặt một đối tượng kim loại như mô tả trên hình 9.14. 
Cấu tạo cảm biến kiểu cảm ứng gồm một cuộn dây đặt trong khung gần một 
nam châm vĩnh cửu như hình 9.14a. Hình.9.14b trình bày hình ảnh đường sức từ 
khi không có đối tượng gần nam châm, đường sức từ phân bố đều, nên không có 
dòng điện chạy trong cuộn dây. Khi có đối tượng ở gần cảm biến, dạng đường 
sức từ bị biến dạng, dòng điện sẽ được tạo ra trong cuộn dây với biên độ và 
dạng sóng phụ thuộc vào mức độ thay đổi từ thông. Hình 9.15 là quan hệ biên 
độ điện áp và khoảng cách giữa cảm biến và đối tượng. Cực tính của điện áp 
sinh ra trong cuộn dây phụ thuộc và chiều đi chuyển của đối tượng so với cảm 
biến. 
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Nam châm Khung 


[Ep<xr TH NHI c> 


Cuộn dây 












Lớp nhựa Đầu nối 


„ Vật thể kim loại 





Đường sức từ 


Hình 9.14. Cảm biến lân cận kiểu cảm ứng: 
a- Sơ đồ cấu tạo; 
b- Biểu diễn đường sức khi không có vật thể; 
c- Biểu diễn đường sức khi có vật thể. 


Biên độ điện áp 


Khoảng cách 


Hình 9.15. Quan hệ biên độ điện áp - khoảng cách. 


9.5.2. Cảm biến lân cận kiểu điện dung 

Cảm biến lân cận kiểu điện dung hoạt động trên hiện tượng thay đổi điện 
dung khi có vật thể dịch chuyển gần cảm biến. Bộ phận chính của cảm biến 
kiểu điện dung như mô tả trên hình 9.15a gồm một tụ điện có hai bản cực: một 
bản cực chuẩn và một bản cực nhạy cảm. Ngoài ra còn có các mạch điện gồm 
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mạch dao động, mạch phát hiện và mạch đầu ra. Bản cực nhạy cảm thông 
thường liên quan đến cơ cấu hay bộ phận công nghệ. Điện dung của tụ điện 
thay đổi khi mức nước thay đổi, hoặc khoảng cách giữa hai bản cực thay đổi. 


AC/C (%) 
» 
Bản cực chuẩn 
l5 
Chất điện 
môi lu Sắt 
4 PVC 
Bản cực nhạy cảm à Z—— 
ụ 5 I0 l§ 
Khoảng cách (mm) 
a) b) 


Hình 9.16. Cảm biến lân cận kiểu điện dung: 
a- Sơ đồ cấu tạo; 
b- Quan hệ điện dung và khoảng cách. 


Có một số phương pháp nhận biết sự thay đổi điện dung của tụ điện. 

Một phương pháp đơn giản là ghép điện dung trong một mạch dao dộng. 
Khi có đối tượng xuất hiện trong vùng kiểm soát của cảm biến, trị số điện dung 
thay đổi và nếu điện dung vượt quá trị số cho phép, mạch bắt đầu dao động. Giá 
trị điện áp dao động là tín hiệu để phát hiện sự có mặt của đối tượng trong vùng 
hoạt động của cảm biến lân cận. Hình 9.16b biểu diễn quan hệ giữa sự thay đổi 
điện dung và khoảng cách. 


9.5.3 Cảm biến quang học 


Cảm biến quang học bao gồm một nguồn sáng thông thường là đèn LED, 
có thể đặt bên ngoài hoặc ở bên trong cảm biến và một bộ phận thu, là một 
phtotranzito. Hình 9.17 mô tả các trường hợp khác nhau ứng với vị trí của đối 
tượng. Khi đối tượng ở xa hoặc ở quá gần nguồn sáng, bộ phận thu sẽ không 
nhận được ánh sáng phản xạ từ đối tượng. Khi đối tượng nằm trong vùng kiểm 
soát của cảm biến, bộ phận thu sẽ nhận được tín hiệu ánh sáng từ đối tượng. 
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Hình 9.17. Cảm biến lân cận quang học. 


9.6. Cảm biến tiếp xúc 


Cảm biến tiếp xúc thường được gắn ở tay robot để nhận biết được có vật thể 
ở các ngón tay robot hoặc để phát hiện vật thể ở trong không gian làm việc. 
Trong một số trường hợp, cảm biến tiếp xúc có thể được sử dụng để điều khiển 
lực tác dụng của tay robot lên vật thể. Có hai loại cảm biến tiếp xúc: Nhị phân 
và tương tự. Cảm biến kiểu nhị phân là các công tắc tiếp xúc sẽ tác động khi có 
vật thể. Cảm biến tương tự sẽ sinh tín hiệu ra tỷ lệ với lực tác dụng. 


9.6.1. Cảm biến nhị phân 


Cảm biến kiểu nhị phân là các công tắc tiếp xúc cỡ nhỏ (microswitch) được 
được đặt ở bề mặt của các ngón tay của bàn tay robot như minh họa trên hình 
9.18. Các tiếp điểm của công tắc sẽ đóng hoặc mở khi có vật thể ở bàn tay 
robot. Các cảm biến tiếp xúc có thể đặt ở mỗi bể mặt ngón tay để nhận biết 
thông tin về vật thể. Các cảm biến cũng có thể được đặt ở mặt ngoài bàn tay để 
tạo tín hiệu điều khiển giúp robot di chuyển tay trong không gian làm việc hoặc 
tạo cho robot có khả năng làm việc trong môi trường hoàn toàn không ánh sáng. 


Công tắc 
cõ nhỏ 






L2 TH.) 


Hình 9.18. Cảm biến tiếp xúc kiểu nhị phân. 
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9.6.2. Cảm biến tương tự 

Cảm biến tương tự là một thiết bị có đầu ra tỷ lệ với lực tác dụng. Một cơ 
cấu đơn giản được vẽ trên hình 9.19, bao gồm một thanh tải có lò xo liên hệ cơ 
khí với một trục quay để có thể dịch chuyển thanh di động khi có lực tác dụng 
lên đầu của thanh. Do có lực tác dụng lên đầu thanh, thanh sẽ dịch chuyển, làm 
bánh răng quay liên tục. Trục bánh răng nối cùng trục vớii một chiết áp. Như 
vậy điện áp ra của chiết áp sẽ tỷ lệ với góc quay của bánh răng và sẽ tỷ lệ với 
lực tác dụng lên thanh tải lò xo. 





Hình 9.19. Cảm biến tiếp xúc tương tự. 
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PHỤ LỤC A 
ĐẠI SỐ MA TRẬN VÀ VECTƠ 


A.1. Định nghĩa 


1. Ma trận có kích thước (mxn) gồm m hàng n cột, với các phần tử là âp 
được viết ở đạng sau: 


ân 8ụ Ân 
a â 8 

A=l ? " 2n (A-1) 
Ñm¡ Âm 3mn 


Nếu m = n, ma trận là ma trận vuông. Nếu n = I, gọi là ma trận cột, hoặc 
gọi la một vectơ a 


2. Ma trận vuông A là ma trận đường chéo nếu 8= 0 với ¡ # j: 


ay 0...0 
0 tuc Đ 

Am cớ cA „.|eếđgfl/6 xay) (A-2) 
00... aự, 


3. Ma trận đường chéo đơn vị là ma trận vuông đường chéo có các hệ số 
ai = 1, và được kí hiệu là [„. Ma trận có tất cả các số hạng bằng O gọi là ma trận 
không (ma trận zêro). 

4. Ma trận chuyển vị của ma trận A (mxn) (A.1) ký hiệu là AT có kích 
thước (nxm) nhận được từ ma trận A bằng chuyển đổi hàng cho cột và ngược lại 


Ân 3p s mg 
AT= 8m Bạy c..c ôn; (A-3) 
8 đạp ... Bạn 


5. Ma trận đối xứng: Ma trận vuông A (nxn) là ma trận đối xứng nếu 


= Á Và ai = ây 
Ân Äj ... Địa 
a a ..: q 
Ai nh (A-4) 
Am Bạn ... Bạn 
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6. Ma trận đối xứng nghịch: Ma trận vuông A là ma trận đôi xửng nghịch 
nếu AT = - A và a, = -a; với ¡ # j và a; = 0: 


0 8;  ... đụ 

-a y 0... 8a, 
A=l|. : SA SỐ (A-5) 

âu Tạ .... 0 


A.2. Phép toán ma trận 


1. Phép cộng ma trận: Hai ma trận A, B có cùng kích thước (mxn), tổng 
của hai ma trận là C: l : 
C=A+B (A-6) 
Với các thành phần của ma trận C là tổng của các thành phần tương ứng của ma 
trận A và B: cụ = au + bụ, 
Một số tính chất: 
A+0=A 
A+B=B+A 
(A+B)+C=A+(B+@ 
2. Phép trừ ma trận: Tương tự như phép cộng hai ma trận: 
C=A-B tới: cụ= ay- bụ 
3. Phép nhân ma trận: Phép nhân ma trận A (mxp) với ma trận B (pxn) có 
kết quả là ma trận C (mxn): 
C=AbB (A-7) 


Với các thành phần của C được tính theo:'c„ = Ÿ*a„ bụ,. 
k=l 


Một số thuộc tính: 
A =Al,=lA 
A(Œ©) =(AB)C 
A(ŒB+O =AB+AC 
(A + BỌC = AC + BC 
(AB) =BTAT 
4. Định thức của ma trận: Định thức của ma trận vuông A (nxn) là một đại 
lượng vô hướng với mọi ¡ = 1...n: 


|A|= Sa C))* Aø| (A-8) 
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Nếu A, B là hai ma trận vuông thì: |AB| =|A||B| 


Định thức con cấp k của ma trận A(mxn) (k <m và k < n) là một định thức 
tạo bởi kỶ phần tử của ma trận nằm ở chỗ giao nhau của k hàng k cột nào đó của 
ma trận. 

3. Ma trận đơn nhất: là ma trận có |A| = 0 

6. Hạng của ma trận r(A): Hạng của ma trận A (mxn) là một số dương lớn 
nhất r sao cho ít nhất tồn tại một định thức con không triệt tiêu CẤP T. 

Một số thuộc tính: 

r(A) < min (m,n) 

1(A) =r(A”) 

r(ATA) = r(A) 

r(AB) < min{r(A),(B)} 
Ma trận A có r(A) = min(m,n) gọi là ma trận có ma trận toàn phần. 
7. Ma trận phụ trợ: Ma trận phụ trợ của ma trận vuông AÀ là ma trận: 

Adj(A) =[(D”/|A g|]* với 4 = 1....n và j = 1....n) 
8. Ma trận nghịch đảo: A'' của ma trận vuông A (nxn) nếu: 

ATA =AA*T =1, 

Ma trận A tồn tại ma trận nghịch đảo nếu: |A| # 0 và ma trận nghịch đảo 
được tính như sau: 








A”= TT (A-9) 
Một số thuộc tính: 

(A''=A 

(Am! = (Amr 

(AB)! = B!A" 


9. Ma trận trực giao: Ma trận vuông A là ma trận trực giao nếu có ma trận 
nghịch đảo bằng ma trận chuyển vị: 
AT=A' (A-10) 
10. Vết của ma trận: Trace(A): Vết của ma trận A là tổng các thành phần 
đường chéo của ma trận A: 


Trace(A) = *a, (A-1D) 
i=l 
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A.3. Vectơ 


1. n vectơ X,(mxI) là độc lập tuyển tính nếu với tất cả các hằng số k, 
phương trình sau được thỏa mãn: 


Điều kiện cần và đủ để các vectơ độc lập tuyến tính là ma trận: 
A2 [RE Xu K„Í 
có hạng là n. 

2. Không gian vectơ: Hệ thống vectơ V là một không gian vectơ trong miền 
giá trì thực nếu tổng hai vectơ bất kỳ trong V và tích một đại lượng vô hướng 
với:một vectơ của V sẽ có giá trị trong V và có các thuộc tính sau: 

X+y=y+Xx với X,ÿœ V 
(X+y)+Z=x+t(y+7) với X,Y,Zze V 
đŨe6X: X+Ö=X với Xe V 


VxeV,3-8)eV: X+(-X)=0 


lx=x với Xe V 
œ(*) = (œB)x với VưÐc R;xeV 
(œ+B)x = ơX+x với VơcR;xXeV 
œ(X+ÿ) =0yÿ +œX với VưeR;x,y<V 
3. Tích vô hướng hai vectơ: Tích vô hướng hai vectơ X ,y có kích thước 
(mx1) được tính: 
Xỹ =XIY( +†X:Y¿+...†X„Y„=X1y=Ky! (A-12) 
Hai vectơ là trực giao khi có tích vô hướng bằng không: 
xy=0 (A-13) 


4. Norm của hai vectơ được định nghĩa như sau: 


|EÌ=xx”x (A-14) 


Một số bất đẳng thức: 
lx+z{< ||+ (A-15) 
#?|<l|+l| (A-16) 


5. Vectơ đơn vị là vectơ có norm của vectơ đó bằng 1: &T§ =1. Cho một 
vectơ X, vectơ đơn vị của nó nhận được bằng chia từng thành phần của vectơ đó 
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cho norm: 
R=~ (A-1?) 
lị 
6. Không gian Eucldiean là không gian vectơ 3 chiều. 
7. Tích có hướng của hai vectơ trong. không gian Euciidean: 


X;Y;› —X;Y; 
XxŸ=| X;y¡ TXIYs (A-18) 
XIY;T—X;ÿy) 
Hoặc viết ở dạng vectơ: 
ï j] k 
it Bi âu, (A-19) 
ŸW\( ŸYy Vy 


=Œ;y; —X;Y;)Ï+ (X;y; —X\Yj)]+ (X\y; —x;y,)k 
Một số thuộc tính: 


=-YxX 
Xx(ÿ+?)=XxY+XxẼ 

Tích có hướng của hai vectơ X,ÿ có thể biểu diễn là tích của toán tử ma 
trận S(X) với vectơ ÿ. Toán tử ma trận S(X)có dạng một ma trận đối xứng 
nghịch: 


Ũ ~X; X; 
SŒ)=| X; 0 -x, (A-20) 
—X;y Xỹ 0 _ 
Xxy=SŒ)y =-S(y)X (A-21) 


Một số thuộc tính: 
S(X)X =-S'(X)X=0 
5(œX +Bÿ)=œS§(X) + BS(ÿ) 
8. Cho 3 vectơ X,ÿ,Z trong không gian Euclidean sẽ có ba tích vô hướng 
Sau; 


X”(ÿ x 5) =ÿ Œ x X) = 5” Œ x ÿ) . 
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9. Hai vectơ trong không gian Euciidean bằng nhau sẽ có: 
X '(xxy)=0 


A.4. Ma trận nghịch đảo giả 


Nghịch đảo của ma trận chỉ được định nghĩa cho ma trận vuông và không 
đơn nhất (định thức ma trận khác không) như A.2. Nghịch đảo của ma trận có 
thể được mở rộng cho ma trận không vuông. 


Xét một ma trận A(mxn) với hạng r(A) = min(m,n). Nếu n < m, ma trận 
nghịch đảo trái được định nghĩa là A, (nxm) sao cho: 
AA=k 
Nếu n >m, ma trận nghịch đảo phải được định nghĩa là A, (nxm) sao cho: 
AyA =1¿ 
Xét trường hợp A(mxn) với m > n và có hạng là n, một ma trận nghịch đảo 
đặc biệt (A ` (axm)) được định nghĩa như sau: 
A2 =(ATAN'AT (A-23) 
Ma trận A* được gọi là ma trận nghịch đảo giả trái vì AZA = I„ Với W, 
(mxm) là ma trận thực dương, ma trận nghịch đảo giả có thể được viết ở dạng: 
A =(ATWIA)!IATW1 (A-24) 
Xét trường hợp ma trận A (mxn) với n > m và có hạng là m, ma trận nghịch 
đảo đặc biệt được định nghĩa như sau: 
Ap=A1A"A}' (A-25) 
Ma trận A‡ được gọi là ma trận nghịch đảo giả phải vì AA,` = I;. Tương tự, 
với W, (nxn) là ma trận thực dương, ma trận nghịch đảo giả có thể được viết ở 
dạng: 
Az=W!AT(AWTATI (A-26) 
Ma trận nghịch đảo giả được sử dụng để giải phương trình ÿ = AX với A là 
ma trận có hạng đủ. Nếu A là ma trận vuông không đơn nhất, để d: ng xác định 
được: 
x=A*y 
Như vậy: A*= A,*= A" 
Nếu A(mxn) với m.< n và có hạng là m, nhận được lời giải không duy nhất: 
X=A!ÿ+-A?A)k 
Với k (nx1) là vectơ bất kỳ và A,* được xác định theo (A-26). 
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PHỤ LỤC B 
HÀM ATAN2 


Hàm atan2 là một hàm vô hướng của hai biến được định nghĩa là góc của 
một số phức: 
atan2(a,b) = arg(b + ja) Œ-D) 
với a, b tương ứng là thành phần ảo và thực của một số phức. 
Một số thuộc tính: 
Ø > atan2(sin9, cosÐ) 
Ø = atan2(ksinÔ, kcosÐ) với k > 0 
atan2(-a, b) = -atan2(a, b) 
1/2 + atan2(a, b) = atan2(b,  a) 
T + atan2(a, b) = atan2(-a, -b) 
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PHỤ LỤC C 
LÝ THUYẾT ỔN ĐỊNH LIAPUNOV 


C.1. ổn định của điểm cân bằng 
Xét một hệ liên tục phi tuyến biến đổi theo thời gian bậc n có dạng: 


x=f(,X), XŒ )=K, (C-1) 
Hoặc hệ gián đoạn phi tuyến biến đổi theo thời gian bậc n có dạng: 
xX(k+l)=f(,xŒ)),  X(Œ,)=x, (C-2) 


trong đó: xeR", teR, keZ. và X,„ là giá trị ban đầu tại t„ (hoặc k, tương ứng). 
1. Trạng thái cân bằng: 
Trạng thái là trạng thái cân bằng khi: 
f(t,X,) =0 với mọi t đối với hệ thống liên tục. 
f(k,X,)= 0 với mọi t đối với hệ thống gián đoạn. 

Không mất tính tổng quát có thể coi X, = 0 là trạng thái cân bằng. 

2. Ổn định và ổn định đều theo nghĩa của Liapunov: 

Trạng thái cân bằng 0 của hệ thống (C-1) là trạng thái ốn định cục bộ theo 
nghĩa Liapunov nếu với mỗi e > 0, sẽ tồn tại một õ(€,t,) sao cho nếu Jx@| <Š 
luôn tồn tại Jx(| <£ với mọi t > t, (đối với hệ thống gián đoạn là k,). 

Nếu Š được chọn độc lập với t, có nghĩa là tổn tại õ(£), trạng thái 0 sẽ la ổn 
định đêu 

3. Ổn định tiệm cận và ổn định tiệm cận đều theo nghĩa của Liapunov: 

Trạng thái 0 là ổn định tiệm cận cục bộ nếu: 

+ Ổn định theo nghĩa Liapunov và 

+ Tồn tại ð(t,) sao cho nếu |X(t)|< ð thì |x 

Trạng thái 0 là ổn định tiệm cận đều (cục bộ) nếu: 

+ Ổn định đều theo nghĩa Liapunov và 

+ Tồn tại ồ` độc lập với t, sao cho nếu |X(©| < õ thì |x(©)| —> 0 khi t — œ. 


—>Ô khi t —» œ. 





262 


4. Ổn định tiệm cận toàn bộ: 

Trạng thái 0 là ổn định tiệm cận toàn bộ nếu ồn định tiệm cận với bất kỳ 
õ'>0. 

5. Ổn định hàm mũ: 

Trạng thái O là ổn định hàm mũ, nếu ổn định theo nghĩa Liapunov và tồn 
tại ỗ” > 0 và một hằng số M < œ và œ > 0 sao cho |xE(ŒJ|< M|x,„|e'*® *) với 


mọi |X(t, )||< ð ; œ gọi là tỷ lệ hội tụ hàm mũ. 


C.2. Tiêu chuẩn ổn định Liapunov cho hệ thống không 
biến đổi theo thời gian (Autonom) 
%ét hàm f của hệ thống liên tục (C-1) không phụ thuộc thời gian tức là: 


X=fŒ®,  K(()=x, (C-3) 
Và hàm f của hệ thống gián đoạn (C-2) không phụ thuộc thời gian: 
XŒ&k+l)=fŒ()),  XŒ,)=x, (C- 


1. Hàm xác định dương (bán xác định đương): 
Một hàm có đạo hàm liên tục V được gọi là xác định dương trong vùng 
UeR" bao gồm điểm gốc nếu: 
a. Ÿ(0)=0, 
b. VŒ) >0,xe U vàx#0 
Một hàm được gọi là bán xác định dương nếu điều kiện b được thay thế 
bằng V(X) > 0,xU. 
2. Định lý Liapunov: 
Đối với một hệ thống không biến đổi theo thời gian, nếu tổn tại một hàm V 
xác định đương có đạo hàm liên tục và tồn tại đạo hàm thảo mãn: 
„ ØVdx ôV„._ = 
V= Ỷ 1 =-WŒ) (C-5) 
Là hàm xác định âm, thì điểm cân bằng 0 là ổn định. Nếu W(&) là hàm 
xác định dương, điểm cân bằng là ổn định tiệm cận. 
Điểm cân bằng 0 là điểm cân bằng ổn định tiệm cận toàn bộ nếu: 
lE[->s =v@0>z. (C6) 


3. Tính ổn định của hệ thống tuyến tính không biến đổi theo thời gian: 
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Điểm cân bằng O0 của hệ thống bậc n; 

X=Ax (C7) 

Là điểm cân bằng ổn định tiệm cận (ổn định hàm mũ) khi: 

a. Tất cả các nghiệm của A nằm bên tráI[ mặt phức. 

b. Với mỗi ma trận Q đối xứng xác định dương bất kỳ, luôn tìm được một 
ma trận P đối xứng xác định dương thỏa mãn phương trình Liapunov sau: 

PA+ATP=-Q (C-8) 

4. Tính ồn định của hệ thống gián đoạn tuyến tính không biến đổi theo thời 
gian: 
Điểm cân bằng 0 của hệ thống bậc n: 

- #K+l)=AXŒ) (C-9) 

Là điểm cần bằng ổn định tiệm cận (ổn định hàm mũ) khi: 

a) Tất cả các nghiệm của A có biên độ nhỏ hơn I. 

b) Với mỗi ma trận Q đối xứng xác định dương bất kỳ, luôn tìm được một 
ma trận P đối xứng xác định dương thỏa mãn phương trình Liapunov gián đoạn 
Sau: 

ATPA-P=-Q (C-10) 
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PHỤ LỤC D 
MỘT SỐ SẢN PHẨM ROBOT CỦA CÁC HÃNG 


D.1. Robot LR Mate 100” (Hãng Fanuc) 

Mô tỉ chung: Gồm 5 trục với hệ 
truyền động điều khiển xecvo, có độ 
chính xác cao, độ tin cậy cao, giao 
điện thân thiện với người sử dụng. 

Ứng dụng: Lắp ráp, bốc dỡ, di 
chuyển, thử nghiệm và lấy mẫu, hàn 
và giáo dục. 


¬ Phạm vi chuyển động 
Hình chiếu bằng của trục 4 





Phạm vi chuyển động 
của truc 4 
##) mttục4 


Hình chiếu cạnh 
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Đặc tính kỹ thuật của Robot LR Mate 100 


Tải trong 3 kg (6.6 p Tải trọng 4 kg (8.8 p 
Phạmv |  |Phạmvi |Tốcđộ  |Phạmvi | Tốcđộ 
chuyển động 150 5/s 
và tốc độ 


|Tre4 |240 | 180% |240 | 100% 


| |Tmes |400đ 2-Ô|250⁄s  |400° ——— |240%s 
K . . . 


D.2. Robot AdeptOne 








Đặc trưng kỹ thuật 


Tải trọng: Chuyển động khớp 
Khối lượng tải: 9.09kg | Khớp 1: 300° 
Mô men quán tính Khớp 2: 294° 
Khớp 4: Khớp 3: Chuẩn: 7,7° 
Chuẩn: 0,028 kgm? Lựachọn: 11,6° 


Lớn nhất: 0,29 kgm? Khớp 4: 554° 
Tham số di chuyển Cơ cấu phanh Trọng lượng: 181,4 kg 
Độ phân dải : 0,0006° Trục 1,2,3 sử dụng cơ cấu | Tốc độ lớn nhất (không tải) 
Độlặplạ : 0,001° phanh ` 1 m/s 
Độ chính xác: 0,003° 
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D.3. Robot IR260 (Hãng ABB) 


Đặc tính kỹ thuật: 
+ Tải trọng : 30 kg. 
tTầmvới : 1,52m. 
+Sốtrục :4 

+ Độ lặp lại : 0,1 mm. 


Thông số chuyển động 


Pham vi làm việc . 


+1800 - -1809 


+ 859 - -289 
+1190- -79 
+300° - -300° 





Tốc độ làm việc 


1539/s 


1759/s 
1539/s 
3429/s 





Kích thước hình học: 


Chiểu caotổng : 1493 mm 

Kích thước bệ : 723 x 600 mm 
Trọng lượng : 340 kg 

Bán kính động cơ : 448 mm 
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